CN-115308770-B - Dynamic obstacle detection method based on fitting graph

CN115308770BCN 115308770 BCN115308770 BCN 115308770BCN-115308770-B

Abstract

The invention discloses a dynamic obstacle detection method based on a fitting graph, wherein a robot is provided with a ranging sensor for acquiring point cloud data; the dynamic obstacle detection method comprises the following steps that a robot extracts a fitting pattern for marking an obstacle from point cloud data, meanwhile, the robot builds a grid map, then the robot compares the center of the fitting pattern with the grid map, screens out the fitting pattern of which the center falls into a passable area of the grid map, and then identifies whether the obstacle marked by the fitting pattern is a dynamic obstacle or not by utilizing the coordinate change of the center of the screened fitting pattern.

Inventors

ZHOU HEWEN
CHEN KEN
XIAO GANGJUN
JIANG LIN
HUANG HUIBAO
CHEN JINJIE
YU CHENGFENG
ZHAO HUI

Assignees

珠海一微半导体股份有限公司
武汉科技大学

Dates

Publication Date: 20260505
Application Date: 20220727

Claims (9)

1. A dynamic obstacle detection method based on a fitting graph is characterized in that the robot is provided with a ranging sensor for acquiring point cloud data, and the dynamic obstacle detection method comprises the following steps: The robot extracts a fitting graph for marking the obstacle from the point cloud data, and simultaneously, the robot constructs a grid map, and then the robot compares the center of the fitting graph with the grid map to screen out the fitting graph of which the center falls into a passable area of the grid map; Then detecting whether the obstacle marked by the fitting graph is a dynamic obstacle or not by utilizing the coordinate change of the center of the screened fitting graph; The dynamic obstacle detection method specifically comprises the following steps: A1, the robot segments the acquired point cloud data to obtain a plurality of point sets; Step A2, combining the point sets obtained in the step A1 to obtain fitting line segments; Step A3, performing a circle fitting operation on the fitting line segments obtained in the step A2 to obtain a preset circumscribed circle, and performing a circle merging process on the preset circumscribed circle to obtain a fitting circle; In step A3, the method for performing the circle merging processing on the preset circumscribed circle includes: When the robot detects that two preset circumscribed circles exist as the containing relation, reserving the preset circumscribed circle with relatively large radius among the two preset circumscribed circles, removing the preset circumscribed circle with relatively small radius among the two preset circumscribed circles, and setting the preset circumscribed circle with relatively large radius among the two preset circumscribed circles as a fitting circle so as to mark or outline an obstacle by the fitting circle; When the robot detects that two preset circumscribed circles are intersected, configuring a connecting line of the centers of the two preset circumscribed circles as a fitted line segment, and then carrying out the circle fitting operation on the fitted line segment to obtain a reference circumscribed circle; when the reference radius is detected to be smaller than the preset allowable radius, the circle center of the reference circumscribed circle is reserved, then the circle center of the reference circumscribed circle is taken as the circle center, the reference radius is taken as the radius to set a circle, and the circle is marked as the fitted circle, so that the fitted circle is marked or an obstacle is marked; a4, constructing a grid map by utilizing point cloud data, performing binarization processing on the grid map according to trafficability, performing corrosion processing on the binarized grid map, and updating the grid map subjected to binarization processing and corrosion processing into the grid map; Step A5, converting the circle center of the fitting circle obtained in the step A3 onto a map to realize comparison with the coordinate index information of the grid map updated finally in the step A4, screening out the fitting circle with the circle center falling into a passable area of the grid map, and determining that the robot obtains the fitting graph for marking the obstacle; And A6, calculating the speed of the obstacle by acquiring the centers of fitting patterns used for marking the obstacle at different moments, and detecting whether the obstacle belongs to a static obstacle or a dynamic obstacle according to the speed of the obstacle.
2. The dynamic obstacle detection method according to claim 1, wherein in the step A1, the point cloud data acquired by the robot through the ranging sensor is a laser point cloud including a plurality of laser points; The method for dividing the acquired point cloud data by the robot to obtain a plurality of point sets comprises the following steps: Step A11, in the process of searching the laser points by the robot, the robot divides the laser point cloud into a plurality of laser point groups by utilizing the change of Euclidean distance of the two laser points; In each laser point group, the robot marks the laser point which is farthest from the fitted line segment which is correspondingly fitted as the farthest point, when the robot calculates that the distance between the farthest point and the fitted line segment which is correspondingly fitted by the laser point group is larger than a preset dividing distance threshold value, the laser point group is divided into two subsets by taking the farthest point as a boundary so that the farthest point becomes the first laser point in one subset, wherein in the laser point group, the robot classifies the laser point with an index value smaller than the farthest point into one subset, and classifies the laser point with an index value larger than the farthest point into the other subset; And A13, the robot updates each subset into the laser point group in the step A12, and then executes the step A12 until the distance between the corresponding furthest point in all the laser point groups and the fitted line segment corresponding to the laser point groups is smaller than or equal to a preset dividing distance threshold value, then the laser point groups with the number of laser points smaller than the preset number threshold value are eliminated, the rest laser point groups are a plurality of point sets in the step A11, and the fitted line segments fitted by the laser points in the existing laser point groups are obtained.
3. The dynamic obstacle detecting method according to claim 2, wherein in the step a11, the method for dividing the laser point cloud into a plurality of laser point groups by the robot using a change in euclidean distance of two laser points includes: Step A111, the robot calculates the Euclidean distance between the laser point searched at the current time and the laser point searched at the last time, if the Euclidean distance is smaller than a preset grouping distance threshold value, the laser point searched at the current time and the laser point searched at the last time are classified into the same laser point group, if the Euclidean distance is larger than or equal to the preset grouping distance threshold value, the laser point searched at the current time is classified into a new laser point group, and the laser point searched at the current time is marked as the first laser point in the new laser point group; Step A112, searching new laser points by the robot, updating the laser points searched at the current time into the laser points searched at the last time, and then executing step A111 until the robot calculates the Euclidean distance between each laser point and any one of the rest laser points, and separating laser point groups according to the Euclidean distance between the two corresponding laser points, and then executing step A12.
4. The method for detecting a dynamic obstacle according to claim 2, wherein in the step A2, the method for merging the point sets includes: And B, after searching two fitting line segments from all the fitting line segments obtained in the step A13 by the robot, if the robot detects that the distance of the nearest endpoint between the two fitting line segments is smaller than a preset contour distance threshold value, the absolute value of the difference value of the slopes of the two fitting line segments is smaller than a preset slope threshold value, and the distance of the intersection point of the extension line of the two fitting line segments and the same coordinate axis is smaller than a preset intercept threshold value, determining that the two fitting line segments are in a straight line, then, combining two laser point groups corresponding to the two fitting line segments into a new laser point group, and fitting the laser points in the combined laser point group into the new fitting line segment by using a least square method.
5. The method of claim 4, wherein for each of the fitted line segments, two end points of the fitted line segment are respectively configured as a first laser point and a last laser point in a laser point group corresponding to the fitted line segment; The distance between the first laser spot and the last laser spot is the maximum value among the distances between any two laser spots within the same laser spot group.
6. The method for detecting a dynamic obstacle according to claim 5, wherein in step A3, the method for performing a circle fitting operation on the fitted line segment includes: The robot obtains a vector which is perpendicular to the fitting line segment and points to the origin of the laser coordinate system from each fitting line segment obtained in the step A2, the angle of an included angle formed by the vector and the abscissa axis is set as a horizontal deflection angle, and the angle of an included angle formed by the vector and the ordinate axis is set as a vertical deflection angle; setting the fitting line segment as one side of an equilateral triangle, and setting the outer centers of the equilateral triangle to be separated from the origin of the laser coordinate system on two sides of the fitting line segment or to be positioned on the same side of the fitting line segment as the origin of the laser coordinate system; The method comprises the steps of setting the product of the length of a fitting line segment and a tangent function value of 30 degrees as the radius of a circumscribing circle of the equilateral triangle, marking the product of half the radius of the circumscribing circle of the equilateral triangle and a cosine function value of a horizontal deflection angle as a preset transverse axis offset coordinate amount, determining that the preset transverse axis offset coordinate amount is the coordinate offset amount of the midpoint of the fitting line segment and the circle center of the circumscribing circle of the equilateral triangle on an abscissa axis, marking the product of half the radius of the circumscribing circle of the equilateral triangle and the cosine function value of a vertical deflection angle as a preset longitudinal axis offset coordinate amount, determining that the preset longitudinal axis offset coordinate amount is the coordinate offset amount of the midpoint of the fitting line segment and the circle center of the circumscribing circle of the equilateral triangle on an ordinate axis, and calculating the coordinate position of the circle center of the circumscribing circle of the equilateral triangle by using the preset longitudinal axis offset coordinate amount and the preset transverse axis offset coordinate amount; on the basis of the coordinate position of the circle center of the circumscribing circle of the equilateral triangle, expanding the radius of the circumscribing circle of the equilateral triangle by a preset radius increment to obtain the preset circumscribing circle, and determining to finish the circle fitting operation on the fitting line segment.
7. The method for detecting dynamic obstacle according to claim 1, wherein the step A4 specifically includes assigning corresponding pixel values to respective grids of the grid map using position information occupied by the obstacle represented by the point cloud data, then marking the grids having the pixel values greater than a preset pixel threshold as passable grids, and marking the grids having the pixel values less than or equal to the preset pixel threshold as non-passable grids, then obtaining a binarized grid map, and determining that the original grid map is subjected to binarization processing; Marking the passable grids of the associated neighborhood of each passable grid in the binarized grid map as the passable grids according to the preset point cloud conversion error to obtain the corroded grid map, wherein the grid region of the associated neighborhood of each passable grid is positively correlated with the grid region related to the preset point cloud conversion error; And (C) updating the grid map which is subjected to binarization processing and corrosion processing successively into the grid map, marking the region formed by the passable grids as a passable region in the grid map, recording the coordinate index value of the passable grids in the grid map, and executing the step A5.
8. The method according to claim 7, wherein in the step A5, the robot projects the center of the fitted circle obtained in the step A3 from the laser coordinate system to the grid map updated in the step A4, and obtains a coordinate index value of the center of the corresponding fitted circle on the global map coordinate system of the grid map; And B, comparing the coordinate index value recorded in the step A4 with the coordinate index value of the circle center of the fitting circle on the global map coordinate system of the grid map, when the coordinate index value of the circle center of the fitting circle obtained in the step A3 on the global map coordinate system of the grid map is equal to one of the coordinate index values recorded in the step A4, determining that the fitting circle is a fitting graph of which the screened center falls into a passable area of the grid map, determining that the screened center falls into the passable area of the grid map, marking the fitting circle as the fitting graph for marking the obstacle, and when the coordinate index value of the circle center of the fitting circle obtained in the step A3 on the global map coordinate system of the grid map is not equal to any one of the coordinate index values recorded in the step A4, removing the circle center of the fitting circle from the grid map.
9. The method for detecting a dynamic obstacle according to claim 8, wherein in the step A6, the method for calculating the speed of the obstacle by obtaining the center of the fitted pattern for marking the obstacle at different moments and identifying whether the obstacle belongs to a static obstacle or a dynamic obstacle according to the speed of the obstacle comprises: the robot marks the coordinates of the centers of the fitting graphs screened in the step A5 at the first moment as first center coordinates, and then marks the coordinates of the centers of the same fitting graphs screened in the step A5 at the second moment as second center coordinates; the robot marks the time difference between the first moment and the second moment as the obstacle movement time difference; the robot marks the distance between the first center coordinate and the second center coordinate as the movement distance of the obstacle; then calculating the ratio of the movement distance of the obstacle to the movement time difference of the obstacle to obtain the speed of the obstacle; When the robot judges that the speed of the obstacle is greater than a preset speed threshold, determining that the obstacle is a dynamic obstacle; when the robot judges that the speed of the obstacle is less than or equal to a preset speed threshold, the obstacle is determined to be a static obstacle.

Description

Dynamic obstacle detection method based on fitting graph Technical Field The invention belongs to the technical field of detection and identification of obstacles, and particularly relates to a dynamic obstacle detection method based on a fitting graph. Background In an indoor home environment, a robot for sweeping floor is easy to touch dynamic obstacles such as people and pets when sweeping floor in a full coverage manner, wherein the robot can be divided into dynamic obstacles and static obstacles according to the state type of the obstacles by a person skilled in the art. When the robot for sweeping floor avoids dynamic obstacles in the environment, the dynamic obstacles are treated as instantaneous static obstacles or the dynamic obstacles cannot be detected, and the obstacles are easily dispersed into scattered points due to the measurement precision of laser, so that the robot is very difficult to judge the dynamic objects, and even erroneous judgment is easy to generate. Disclosure of Invention Aiming at the problem of obstacle detection in a motion state, the invention discloses a dynamic obstacle detection method based on a fitting graph, which comprises the following specific technical scheme: A dynamic obstacle detection method comprises the steps that a robot is provided with a ranging sensor and used for acquiring point cloud data, the dynamic obstacle detection method comprises the following steps of extracting a fitting pattern used for marking an obstacle from the point cloud data, meanwhile, constructing a grid map by the robot, comparing the center of the fitting pattern with the grid map by the robot, screening out the fitting pattern of which the center falls into a passable area of the grid map, and detecting whether the obstacle marked by the fitting pattern is a dynamic obstacle or not by utilizing the coordinate change of the center of the screened fitting pattern so as to distinguish static obstacle from dynamic obstacle. The method comprises the steps of A1, dividing acquired point cloud data by a robot to obtain a plurality of point sets, A2, combining the point sets obtained in the step A1 to obtain a fitting line segment, A3, carrying out circle fitting operation on the fitting line segment obtained in the step A2 to obtain a preset circumcircle, combining the preset circumcircle to obtain a fitting circle, A4, constructing a grid map by utilizing the point cloud data, carrying out binarization processing on the grid map according to the trafficability, carrying out corrosion processing on the binarized grid map, then updating the grid map subjected to the binarization processing and the corrosion processing into a grid map, A5, converting the circle center of the fitting circle obtained in the step A3 onto the map to realize comparison with coordinate index information of the grid map which is updated in the step A4, screening out circles of which the circle center falls into a passable area of the grid map, determining that the robot obtains the fitting graph for marking the obstacle, A6, and calculating whether the static obstacle is the obstacle or not at the same time by obtaining the static obstacle, and identifying whether the obstacle belongs to the dynamic obstacle according to the speed of the obstacle. Further, in the step A1, the point cloud data acquired by the robot through the ranging sensor is a laser point cloud, and the laser point cloud includes a plurality of laser points; the method for obtaining a plurality of point sets by the robot includes the steps of A11, dividing the laser point cloud into a plurality of laser point groups by the robot by utilizing the Euclidean distance change of two laser points in the process of searching the laser points by the robot, A12, fitting the laser points in each laser point group into a fitting line segment by the robot by utilizing a least square method, marking the laser point which is farthest from the fitting line segment which is correspondingly fitted in each laser point group as the most distant point by the robot, dividing the laser point group into two subsets by taking the most distant point as a boundary when the distance between the most distant point and the fitting line segment which is correspondingly fitted in the laser point group is larger than a preset dividing distance threshold value, dividing the laser point group into the first laser point in one subset by taking the most distant point as the boundary, classifying the laser points in the laser point group into one subset by the robot, classifying the laser points with index values smaller than the most distant point into the subset by the laser points, and marking the laser points with the index values larger than the most distant point in each laser point group by the robot, updating the laser points in the step A13 or the laser point group with the number which is smaller than the laser points in the preset threshold value, and the step A is car