A dynamic obstacle detection method based on fitted graphs

Abstract

The application discloses a dynamic obstacle detection method based on fitting graphs. A robot is provided with a ranging sensor for obtaining point cloud data. The dynamic obstacle detection method comprises the following steps: the robot extracts fitting graphs for marking obstacles from the point cloud data; meanwhile, the robot constructs a grid map; then the robot compares the center of the fitting graphs with the grid map, and screens out the fitting graphs whose center falls into the passable area of the grid map; then the coordinate change of the center of the screened fitting graphs is used to identify whether the obstacle marked by the fitting graphs is a dynamic obstacle.

Description

Technical Field

[0001] This invention belongs to the technical field of obstacle detection and recognition, specifically relating to a dynamic obstacle detection method based on fitted graphics. Background Technology

[0002] In indoor home environments, robotic vacuum cleaners often encounter dynamic obstacles such as people and pets when performing full-coverage cleaning. These obstacles can be categorized as dynamic or static based on their state. When avoiding dynamic obstacles, robotic vacuum cleaners often treat them as momentarily static or fail to detect them altogether. Furthermore, due to the limited accuracy of laser measurements, obstacles are easily scattered into individual points, making it very difficult for the robot to judge dynamic objects and potentially leading to misjudgments. Summary of the Invention

[0003] To address the problem of obstacle detection in motion, this invention discloses a dynamic obstacle detection method based on fitted graphics, the specific technical solution of which is as follows:

[0004] A dynamic obstacle detection method includes a robot equipped with a ranging sensor to acquire point cloud data. The method comprises the following steps: the robot extracts a fitted graphic for marking obstacles from the point cloud data; simultaneously, the robot constructs a grid map; then, the robot compares the center of the fitted graphic with the grid map, and selects fitted graphics whose centers fall within a passable area of ​​the grid map; then, the robot uses the coordinate changes of the center of the selected fitted graphic to detect whether the obstacle marked by the fitted graphic is a dynamic obstacle, thereby distinguishing between static and dynamic obstacles.

[0005] Further, step A specifically includes: Step A1, the robot segments the acquired point cloud data to obtain several point sets; Step A2, the point sets obtained in step A1 are merged to obtain fitted line segments; Step A3, the fitted line segments obtained in step A2 are subjected to circle fitting to obtain a preset circumcircle; then the preset circumcircle is merged to obtain a fitted circle; Step A4, a raster map is constructed using the point cloud data, then the raster map is binarized according to its accessibility, then the binarized raster map is eroded, and finally the binarized raster map is... The raster map after processing and erosion is updated to a raster map; Step A5: The center of the fitted circle obtained in Step A3 is transferred to the map to compare with the coordinate index information of the raster map last updated in Step A4, and the fitted circles whose centers fall into the passable area of ​​the raster map are selected, and the robot obtains the fitted graphic used to mark obstacles; Step A6: By obtaining the center of the fitted graphic used to mark obstacles at different times, the speed of the obstacle is calculated, and then the obstacle is identified as a static obstacle or a dynamic obstacle based on the speed of the obstacle.

[0006] Further, in step A1, the point cloud data acquired by the robot through the ranging sensor is a laser point cloud, which includes multiple laser points. The method by which the robot segments the acquired point cloud data to obtain several point sets includes: Step A11, during the robot's search for laser points, the robot uses the change in the Euclidean distance between two laser points to divide the laser point cloud into multiple laser point groups; Step A12, the robot uses the least squares method to fit a fitted line segment to the laser points in each laser point group; within each laser point group, the robot marks the laser point farthest from the fitted line segment as the farthest point. When the robot calculates that the distance between the farthest point and the fitted line segment corresponding to that laser point group is greater than a preset segmentation distance threshold, then the farthest point is used as the boundary to divide the laser point group into multiple laser point groups. The laser point group is divided into two subsets so that the farthest point becomes the first laser point in one subset. Within the laser point group, the robot assigns laser points with index values ​​smaller than the farthest point to one subset and laser points with index values ​​larger than the farthest point to the other subset. Step A13: The robot updates each subset to the laser point group described in step A12 and executes step A12 again until the distance between the farthest point in all laser point groups and the fitted line segment corresponding to the laser point group is less than or equal to a preset segmentation distance threshold. Then, laser point groups with fewer than a preset number threshold are removed. The remaining laser point groups are the several point sets described in step A11, and the fitted line segments fitted by the laser points in each existing laser point group are obtained.

[0007] Further, in step A11, the method by which the robot divides the laser point cloud into multiple laser point groups using the change in the Euclidean distance between two laser points includes: Step A111: The robot calculates the Euclidean distance between the currently searched laser point and the previously searched laser point; if the Euclidean distance is less than a preset grouping distance threshold, the currently searched laser point and the previously searched laser point are grouped into the same laser point group; if the Euclidean distance is greater than or equal to the preset grouping distance threshold, the currently searched laser point is grouped into a new laser point group, and the currently searched laser point is marked as the first laser point in the new laser point group; then step A112 is executed; Step A112: The robot searches for new laser points, then updates the currently searched laser point to the previously searched laser point, and then executes step A111 again, until the robot has calculated the Euclidean distance between each laser point and any other laser point and divided the laser point groups according to the Euclidean distance between the corresponding two laser points, and then executes step A12.

[0008] Further, in step A2, the method for merging the point set includes: whenever the robot searches for two fitted line segments from all the fitted line segments obtained in step A13, if the robot detects that the distance between the nearest endpoints of the two fitted line segments is less than a preset contour distance threshold, and the absolute value of the difference in the slopes of the two fitted line segments is less than a preset slope threshold, and the distance between the intersection points of the extensions of the two fitted line segments and the same coordinate axis is less than a preset intercept threshold, then it is determined that the two fitted line segments are on a straight line; then, the two laser point groups corresponding to the two fitted line segments are merged into a new laser point group, and the least squares method is used to fit the laser points in the merged laser point group into a new fitted line segment.

[0009] Furthermore, for each fitted line segment, the two endpoints of the fitted line segment are respectively configured as the first laser point and the last laser point in the laser point group corresponding to the fitted line segment; each laser point group corresponds to a fitted line segment; within the same laser point group, the distance between the first laser point and the last laser point is the maximum value among the distances between any two laser points.

[0010] Further, in step A3, the method for performing a circle fitting operation on the fitted line segment includes: the robot obtaining a vector perpendicular to the fitted line segment and pointing to the origin of the laser coordinate system from each fitted line segment obtained in step A2; setting the angle between this vector and the horizontal axis as the horizontal deflection angle; setting the angle between this vector and the vertical axis as the vertical deflection angle; then setting the fitted line segment as one side of an equilateral triangle; then setting the circumcenter of the equilateral triangle to be on opposite sides of the origin of the laser coordinate system from the fitted line segment, or on the same side as the origin of the laser coordinate system from the fitted line segment; then setting the product of the length of the fitted line segment and the tangent function value of 30 degrees as the radius of the circumcircle of the equilateral triangle; then setting the radius of the circumcircle of the equilateral triangle as half the radius of the circumcircle and the cosine function of the horizontal deflection angle... The product of the values ​​is marked as a preset horizontal axis offset coordinate, and this preset horizontal axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the horizontal axis. Simultaneously, the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the vertical deflection angle is marked as a preset vertical axis offset coordinate, and this preset vertical axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the vertical axis. Then, the coordinate position of the center of the circumcircle of the equilateral triangle is calculated using the preset vertical axis offset coordinate and the preset horizontal axis offset coordinate. Based on the coordinate position of the center of the circumcircle of the equilateral triangle, the radius of the circumcircle of the equilateral triangle is increased by a preset radius increment to obtain the preset circumcircle, and the circle fitting operation is completed for the fitted line segment.

[0011] Further, in step A3, the method for merging the preset circumcircles includes: when the robot detects that two preset circumcircles are contained within each other, the robot retains the preset circumcircle with the larger radius and discards the preset circumcircle with the smaller radius. The preset circumcircle with the larger radius is then set as the fitting circle, so that the fitting circle marks or delineates an obstacle. When the robot detects that two preset circumcircles intersect, the line connecting the centers of the two preset circumcircles is configured as a fitting line segment. The fitting line segment is then subjected to the aforementioned circle fitting operation to obtain a reference circumcircle. The radius of the reference circumcircle is then added to the larger radius of the two preset circumcircles before the aforementioned circle fitting operation to obtain a reference radius. When the reference radius is detected to be smaller than a preset allowable radius, the center of the reference circumcircle is retained. A circle is then set with the center of the reference circumcircle as the center and the reference radius as the radius, and this circle is marked as the fitting circle, so that the fitting circle marks or delineates an obstacle.

[0012] Further, step A4 specifically includes: using the location information of obstacles represented by point cloud data, assigning corresponding pixel values ​​to each grid of the grid map; then marking grids with pixel values ​​greater than a preset pixel threshold as passable grids, and marking grids with pixel values ​​less than or equal to the preset pixel threshold as impassable grids, thereby obtaining a binarized grid map, and confirming that the original grid map has undergone binarization processing; then, according to a preset point cloud conversion error, marking the passable grids in the associated neighborhood of each impassable grid in the binarized grid map as impassable grids, thereby obtaining an eroded grid map; wherein, the grid region in the associated neighborhood of each impassable grid is positively correlated with the grid region involved in the preset point cloud conversion error; then, updating the grid map after binarization and erosion processing to the grid map, marking the region composed of passable grids as the passable region in the grid map, and recording the coordinate index values ​​of the passable grids in the grid map; then executing step A5.

[0013] Further, in step A5, the robot projects the center of the fitted circle obtained in step A3 from the laser coordinate system onto the grid map last updated in step A4, obtaining the coordinate index value of the center of the corresponding fitted circle in the global map coordinate system of the grid map; then, it compares the coordinate index value recorded in step A4 with the coordinate index value of the center of the fitted circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitted circle obtained in step A3 in the global map coordinate system of the grid map is equal to one of the coordinate index values ​​recorded in step A4, it is determined that the fitted circle is the fitted graphic whose center falls into the passable area of ​​the grid map selected in step A, and the fitted circle whose center falls into the passable area of ​​the grid map is selected and marked as the fitted graphic used to mark obstacles; when the coordinate index value of the center of the fitted circle obtained in step A3 in the global map coordinate system of the grid map is not equal to any of the coordinate index values ​​recorded in step A4, the center of the fitted circle is removed from the grid map.

[0014] Further, in step A6, the method of obtaining the center of the fitted graph used to mark the obstacle at different times, calculating the speed of the obstacle, and then identifying whether the obstacle is a static obstacle or a dynamic obstacle based on the speed of the obstacle includes: the robot marks the coordinates of the center of the fitted graph selected in step A5 at the first time as the first center coordinates; then, the robot marks the coordinates of the center of the same fitted graph selected in step A5 at the second time as the second center coordinates; the robot marks the time difference between the first time and the second time as the obstacle movement time difference; the robot marks the distance between the first center coordinates and the second center coordinates as the obstacle movement distance; then, the ratio of the obstacle movement distance to the obstacle movement time difference is calculated to obtain the speed of the obstacle; when the robot determines that the speed of the obstacle is greater than a preset speed threshold, the obstacle is determined to be a dynamic obstacle, and steps A1 to A6 are repeated to track the movement state of the dynamic obstacle; when the robot determines that the speed of the obstacle is less than or equal to the preset speed threshold, the obstacle is determined to be a static obstacle.

[0015] The beneficial technical effects of this invention are as follows: This invention groups point cloud data, further subdivides the initially divided laser point groups, performs circle fitting on the fitted line segments fitted to each laser point group, and then merges the circumcircles obtained from the circle fitting operation to obtain a fitted circle. The movement speed of the center of the fitted circle used to mark the same obstacle is then used to determine whether the obstacle marked by the fitted circle is a dynamic obstacle. This not only enables the detection of the state type of the obstacle marked by the fitted circle but also allows the construction of a motion state model of the obstacle. This invention merges the newly fitted line segments to combine fitted line segments located on the same straight line, at least two adjacent fitted line segments or two adjacent laser point groups, and merges the preset circumcircles obtained based on these fitted line segments. This avoids two fitted circles with different center positions or radii representing the same obstacle, improving the accuracy of dynamic obstacle detection.

[0016] This invention performs a circle fitting operation on the fitted line segment to obtain a preset circumcircle, and then performs circle merging to obtain a fitted circle, which is equivalent to a circular obstacle, rather than simply using fitted line segments to represent dynamic obstacles. This reduces the interference of static obstacles such as long corridors and walls on detection. Moreover, to ensure that the generated circle can fully represent or even delineate the obstacle, the radius of the circumcircle of the equilateral triangle needs to be expanded by a finite preset radius increment. At the same time, the radius of dynamic obstacles in the actual environment is not very large. To eliminate the influence of long corridors, walls, and large obstacles, a preset allowable radius is set. Only preset circumcircles or fitted circles with radii smaller than this preset allowable radius can be retained. Attached Figure Description

[0017] Figure 1 This is a flowchart of a dynamic obstacle detection method disclosed in one embodiment of the present invention.

[0018] Figure 2 This is a schematic diagram of dividing a laser point cloud into two laser point groups, as disclosed in one embodiment of the present invention.

[0019] Figure 3 This is a schematic diagram of the segmentation of a laser dot group disclosed in one embodiment of the present invention.

[0020] Figure 4 This is a schematic diagram of the merging process of two fitted line segments disclosed in one embodiment of the present invention.

[0021] Figure 5 This is a schematic diagram of the circumcircle of an equilateral triangle disclosed in one embodiment of the present invention.

[0022] Figure 6 This is a schematic diagram of an embodiment of the present invention, which discloses a process of merging two preset circumscribed circles into a fitted circle. Detailed Implementation

[0023] To facilitate understanding and implementation of the present invention by those skilled in the art, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the embodiments described herein are for illustration and explanation only and are not intended to limit the present invention. The technical solutions of the present invention will be further described in detail below through embodiments and with reference to the accompanying drawings.

[0024] This invention discloses a dynamic obstacle detection method based on fitted graphics. The main body executing the dynamic obstacle detection method is an automatically moving robot. The robot is equipped with a ranging sensor to acquire point cloud data. This point cloud data is the result of scanning the robot's environment with modulated light signals emitted by the ranging sensor. Specifically, it is information reflected by obstacles in the robot's environment, reflecting the positional information of the entire or partial outline of the obstacles detected by the robot. Preferably, the ranging sensor is a lidar that supports 360-degree rotation, acquiring laser point clouds in various angular directions, i.e., the point cloud data, which consists of rotation angle information and distance information; generally, the lidar acquires one frame of laser point cloud for each rotation.

[0025] As one embodiment, the dynamic obstacle detection method includes: the robot extracting a fitted graphic for marking obstacles from point cloud data, i.e., constructing a graphic that can represent the obstacle in the point cloud data, preferably a circle surrounding (or enclosing) the obstacle to reduce the interference of static obstacles such as long corridors and walls on detection. Simultaneously, the robot constructs a grid map, which can be simplified to a binary grid map composed of grids represented by 0s and 1s, or it can be a probabilistic grid map; wherein, the fitted graphic for marking obstacles is not necessarily located within the grid map. Therefore, the robot needs to compare the center of the fitted graphic with the grid map, filtering out fitted graphics whose centers fall within the passable area of ​​the grid map. Specifically, the robot first transforms the center of the fitted graphic onto the grid map, and uses image processing methods to eliminate fitted graphics whose centers are on the map boundary, thereby filtering out fitted graphics whose centers fall within the passable area of ​​the grid map, and obtaining the location information of possible dynamic obstacles within the grid map.

[0026] The robot then uses the coordinate changes of the center of the previously selected fitted graph to detect the state type of the obstacle marked by the fitted graph, in order to accurately distinguish between static and dynamic obstacles. The fitted graph is configured as the overall information of the obstacle, which more effectively identifies the obstacle's state than discrete points, especially since the coordinate changes of the center of the fitted graph are configured as the obstacle's motion information. The coordinate changes of the center of the selected fitted graph represent the coordinate motion of the fitted graph used to mark the same obstacle (a moving obstacle). In this embodiment, the point cloud data acquired by the robot is always reflected from the same obstacle, which may be in motion. The coordinates of the center of the fitted graph used to mark this obstacle continuously change over a period of time, forming the obstacle's motion speed. Therefore, this embodiment determines whether an obstacle is a dynamic obstacle by detecting changes in the obstacle's motion speed, thereby improving the detection accuracy of dynamic obstacles and making it better suited for local obstacle avoidance, including avoiding moving obstacles within a local area or on a local path.

[0027] As one example, such as Figure 1As shown, the dynamic obstacle detection method specifically includes: Step A1, the robot segments the acquired point cloud data to obtain several point sets; wherein, the point cloud data specifically refers to the position information of the obstacle point cloud. When the point cloud data is represented as laser data (laser point cloud), each frame of laser data has the angle of a corresponding laser and the distance of the laser point, expressed in polar coordinates. After determining the robot's own pose, the laser data is transformed by trigonometric functions to obtain the coordinate position information of the obstacle. Generally, after acquiring the laser data of the current frame through the robot's lidar, laser data with excessively far distances are removed, and all laser points that meet the threshold range requirements are retained in the coordinate form of the laser coordinate system to form a laser point cloud. The greater the distance of the laser points, the sparser the laser point cloud. Then, the robot groups the acquired point cloud data, and further segments each group of point cloud data, repeating this process until each laser point is segmented into a corresponding group, obtaining several point sets.

[0028] It should be noted that each set of points can be fitted with a corresponding line segment, i.e., a straight line segment. Within the allowable error range, the two endpoints of the fitted straight line segment can be represented by the first and last points in the set of points, respectively. The first and last points in the set of points are the points with the smallest and largest index values, respectively, which are the two points farthest apart in the set of points. Therefore, the straight line segment fitted for each set of points can be described by the corresponding line equation, the first point, and the last point in the set of points.

[0029] Step A2: The robot merges the point set obtained in Step A1 (since all laser points that meet the threshold range requirements need to be grouped, the point set obtained in Step A1 can be understood as the final point set obtained in Step A1). That is, it merges all the point sets repeatedly segmented in Step A1 to obtain a new point set and obtains the fitted line segments, including the fitted line segments corresponding to the merged point sets and the fitted line segments corresponding to the point sets that do not need to be merged and belong to the point sets segmented in Step A1. To avoid excessive segmentation of the point cloud data in Step A1, resulting in too many fitted line segments, Step A2 judges and merges the fitted line segments in turn, which is also reflected in the merging of the corresponding point sets to obtain a relatively small number of fitted line segments. When there are some fitted line segments that do not need to be merged, they are all included in the fitted line segments obtained in Step A2, along with the fitted line segments merged in Step A2, as the final fitted line segments obtained in Step A2, each corresponding to a point set.

[0030] Step A3: The robot performs circle fitting on the fitted line segments obtained in Step A2. This involves performing circle fitting on each set of points after the merging process in Step A2 and on each set of points before the merging process in Step A2 to obtain a preset circumcircle. This is used to fully mark and delineate the corresponding obstacles, including dynamic and static obstacles. However, in cases where different preset circumcircles may mark the same object, the currently obtained preset circumcircles need to be merged to replace the previously obtained preset circumcircles. The fitted circle obtained through the circle merging process becomes the fitted circle obtained in Step A3. Each preset circumcircle obtained through the circle fitting operation needs to undergo circle merging. In real-world environments, the radius of dynamic obstacles is not very large. To eliminate the influence of long corridors, walls, and large obstacles, this embodiment only considers performing circle fitting on the fitted line segments to obtain preset circumcircles, and then merging them to obtain the fitted circle, which is equivalent to a circular obstacle, rather than simply using fitted line segments to represent dynamic obstacles. Generally, line segment obstacles are used to represent walls or long corridors.

[0031] Step A4: Construct a raster map using point cloud data. At this point, the point cloud data needs to be converted into map data first. Then, the raster map is binarized according to its accessibility so that each grid cell is represented by binary 0 or binary 1. The binarized raster map is then eroded. Finally, the raster map after binarization and erosion is updated to a raster map, thereby representing the raster map in a more unified and convenient form for image calculation operations.

[0032] It should be noted that step A4 can be configured to be executed synchronously with steps A1, A2, or A3. These are independent steps, meaning the grid map constructed in step A4 is not associated with the point set segmented in step A1, the fitted line segments obtained in step A2, or the fitted circles obtained in step A3. In step A4, the robot uses the Cartographer algorithm to construct the grid map. The Cartographer algorithm primarily processes data from LiDAR, IMU, and odometry, and constructs the map based on this data. In LiDAR positioning and navigation scenarios, the Cartographer framework is generally divided into front-end matching and back-end optimization tasks. Firstly, the front-end matching process is essentially the creation of a local map. This involves adding a series of point cloud filters and gyroscope and odometer data for pose fusion, and then adding the fused pose to the local map. The matching strategy here is to match the pose of the currently scanned laser frame image with the pre-scanned laser frame image. However, when a large number of laser frame images are created, errors accumulate. Therefore, back-end loop closure detection is introduced for optimization, which is the branch and bound algorithm. This algorithm divides the grid into several depth levels (in layman's terms, several resolution levels) and matches the low-resolution data first, then the high-resolution data, greatly shortening the loop closure detection time. Once the loop is established, optimization is performed. The optimization method involves constructing a pose graph by creating constraints from gyroscope data, odometry data, laser data, and local map data to establish a polygonal loop. Nodes on the loop are then created, and optimization is performed using a sparse pose graph. The optimization method involves establishing a nonlinear least squares equation for optimization. Each grid cell in the Cartographer-built map is then assigned a corresponding pixel value to describe the actual environment distribution, including the distribution of obstacles. Finally, the original grid map is obtained, which is then subjected to the aforementioned binarization and erosion processing.

[0033] Step A5: The robot transfers the center of the fitted circle obtained in step A3 (all preset circumscribed circles obtained by the robot in step A3 need to be merged into a fitted circle) onto the map to compare with the coordinate index information of the grid map updated in step A4. Fitted circles whose centers fall into the passable area of ​​the grid map are selected to eliminate fitted circles at the grid map boundary. In some embodiments, known obstacles can be eliminated during the comparison with the grid map, and newly added obstacles can be marked with circles. The robot obtains the fitted graphic used to mark obstacles, that is, it obtains the position information of possible dynamic obstacles inside the grid map. The fitted graphic is represented in the form of a fitted circle (circle).

[0034] Step A6: By obtaining the center of the fitted graph used to mark the obstacle at different times, the speed of the obstacle is calculated, and then the obstacle is identified as either a static obstacle or a dynamic obstacle based on its speed. The fitted graph used to mark part or all of the same obstacle changes with the corresponding fitted line segment, which in turn changes with the point set. This change refers to the change in the position of the same obstacle reflected in the point cloud data, describing the obstacle's motion state. In this embodiment, the fitted graph used to mark part or all of the same obstacle can be represented as a fitted circle delineating part or all of the obstacle's outline; in most cases, it is a circle delineating the entire outline of the obstacle. Thus, the dynamic obstacle is determined by the real-time change of the center of the fitted circle selected in step A5.

[0035] Combining steps A1 to A6 above, the robot groups the point cloud data, further segments the initially divided laser point groups, and performs circle fitting on the fitted line segments fitted to each laser point group. Then, it merges the circumcircles obtained from the circle fitting operation to obtain a fitted circle. The robot then determines whether the obstacle marked by the fitted circle is a dynamic obstacle by the movement speed of the center of the fitted circle used to mark the same obstacle. This allows for both the detection of the state type of the obstacle marked by the fitted circle and the construction of a motion state model of the obstacle. This invention merges the newly fitted line segments to combine fitted line segments located on the same straight line, at least two adjacent fitted line segments or two adjacent laser point groups. It also merges the preset circumcircles obtained based on these fitted line segments, avoiding the possibility of two fitted circles with different center positions or radii representing the same obstacle, thus improving the accuracy of dynamic obstacle detection.

[0036] Preferably, the point cloud data acquired by the robot through the ranging sensor is a laser point cloud, which includes multiple laser points, all of which can be searched in the robot's memory. These laser points can be arranged in the robot's memory according to the position of obstacle reflection, so that two adjacent laser points are two adjacent laser points, and one laser point can be regarded as being in the neighborhood of another laser point. In some embodiments, the laser point cloud can also be sorted in the robot's memory according to the time sequence of the robot's search.

[0037] The method by which the robot segments the acquired point cloud data to obtain several point sets includes:

[0038] Step A11: During the robot's search for laser points, the robot uses the change in the Euclidean distance between two laser points to divide the laser point cloud into multiple laser point groups. Specifically, the method for the robot to divide the laser point cloud into multiple laser point groups using the change in the Euclidean distance between two laser points includes: Step A111: The robot calculates the Euclidean distance between the currently searched laser point and the previously searched laser point; if the Euclidean distance is less than a preset grouping distance threshold, the currently searched laser point and the previously searched laser point are grouped into the same laser point group; if the Euclidean distance is greater than a certain threshold, the robot calculates the Euclidean distance between the currently searched laser point and the previously searched laser point; if the Euclidean distance is greater than a certain threshold, the robot calculates the Euclidean distance between the currently searched laser point and the previously searched laser point. If the distance is equal to or greater than the preset grouping distance threshold, the laser point found in the current search is assigned to a new laser point group, and the laser point found in the current search is marked as the first laser point in the new laser point group; then step A112 is executed; in step A112, the robot searches for new laser points, then updates the laser point found in the current search to the laser point found in the previous search, and then executes step A111, until the robot has calculated the Euclidean distance between each laser point and any other laser point and divided the laser point group according to the Euclidean distance between the corresponding two laser points, and then executes step A12.

[0039] As one embodiment, corresponding to step A11, such as Figure 2 As shown, the robot calculates the current laser point sequentially. Compared to the previous laser point European distance Where i is the sequence number of the laser point in the memory space; in some embodiments, the current laser point It is the previous laser point Laser points within the vicinity of; if the Euclidean distance If the distance is less than the preset grouping threshold, then the laser point will be... and laser point Group them into the same group and mark them as two adjacent laser points within the same laser point group; otherwise, use... Create a new laser point group S for the first point. j Relative to the laser point The laser point group S it is located in j-1 In other words, the preset grouping distance threshold is used to represent the maximum distance that can be reached between two laser points within the same pre-configured group of laser points, or a distance greater than this maximum distance. After the robot has calculated the Euclidean distance between any two laser points (i.e., calculated the Euclidean distance between each laser point and any other laser point and grouped the laser points according to the corresponding Euclidean distances), the robot has traversed all the laser points and cannot expand to any new laser points, resulting in several grouped laser point groups S. j (j=1,2,...,n,n≤m), where j represents the group number or sequence number of the laser point group; it should be noted that, Figure 2In China, with The format retains the laser points acquired by the robot, or the laser points that meet the requirements among the acquired laser points, x i This corresponds to the x-coordinate of the i-th laser point in the laser coordinate system, y i This corresponds to the ordinate of the i-th laser point in the laser coordinate system, where m is the total number of laser points. m is also equivalent to the number of laser points that the robot needs to search. Figure 2 Among the laser points, laser point group S j The offset distance of each laser point within the laser array relative to the origin (ox, oy) along the y-axis is less than that of the laser point group S. j-1 The offset distance of the laser point within the range relative to the origin (ox, oy) in the y-axis direction.

[0040] Step A12: The robot uses the least squares method to fit a fitted line segment to the laser points in each laser point group. In this embodiment, each laser point group corresponds to a fitted line segment. Within each laser point group, the robot marks the laser point farthest from the fitted line segment as the farthest point. When the robot calculates that the distance between the farthest point and the fitted line segment of the laser point group is greater than a preset segmentation distance threshold, the laser point group is divided into two subsets with the farthest point as the boundary, so that the farthest point becomes the first laser point in one of the subsets, and the laser point group is determined to meet the segmentation condition. The preset segmentation distance threshold is used to represent... The maximum distance from a laser point within the same group of laser points to its corresponding fitted line; within this group of laser points, the robot assigns laser points with index values ​​smaller than the farthest point to one subset and laser points with index values ​​larger than the farthest point to another subset. The farthest point can be assigned to either of the two subsets or simultaneously to each of the two subsets; in some embodiments, the robot assigns laser points with search times earlier than the farthest point to one subset and laser points with search times later than the farthest point to another subset. The robot searches / traverses laser points sequentially according to their proximity.

[0041] Step A13: The robot updates each subset to the laser point group described in step A12, and then executes step A12 again until the distance between the farthest point in each laser point group and the fitted line segment corresponding to that laser point group is less than or equal to a preset segmentation distance threshold, that is, all the separated laser point groups or subsets do not meet the segmentation conditions; then, laser point groups with fewer than a preset number threshold are removed, and the remaining laser point groups are several point sets described in step A11, and the fitted line segments fitted by the laser points in each existing laser point group are obtained. The preset number threshold is preferably 3.

[0042] As one example, such as Figure 3 As shown, the robot first uses the least squares method to segment the laser point group S to be segmented. j A fitted line segment l is obtained within the laser point in the image. m That is, laser point group S j Including the laser point, the equation of the straight line A is obtained by fitting a straight line using the least squares method. m *x+B m *y+C m =0, since the line being sought is not unique, we only need to set C here. m As long as it is not equal to 0, the linear coefficient A can be determined using the least squares method. m and B m Where, the linear coefficient A m linear coefficient B m and linear coefficient C m For the specific solution method, please refer to the calculation formula related to line fitting disclosed in step 5 of claim 2 of Chinese invention patent CN113253717A, which will not be repeated here. Then, find the laser point group that is far from the line l. m The farthest laser point If the laser point to straight line segment l m distance d k If the distance exceeds the preset segmentation threshold, then the laser will be applied from that laser point. This laser point group is divided into two subsets S. j S j+1 , where the subset S j There exists Subset S j+1 There exists and It is simultaneously included in the subset S j and into subset S j+1 Based on this, the aforementioned segmentation process is repeated for each subset until none of the subsets meet the segmentation conditions. Then, the robot removes laser point groups with fewer than 3 laser points, and the corresponding fitted line segments are not used by the robot for subsequent circle fitting operations. Furthermore, all point cloud data can be separated into several point sets according to the rules set in the aforementioned embodiment, with each point set corresponding to a fitted straight line. In this embodiment, the first and last laser points in a laser point group, along with the corresponding fitted line equation, are used to describe the fitted line segment. Within the allowable error range, the first and last laser points in a laser point group can be considered as the two endpoints of the fitted line segment corresponding to that laser point group.

[0043] As one embodiment, in step A2, the method for merging the point set includes: whenever the robot finds two fitted line segments from the fitted line segments finally obtained in step A13, detecting the attribute differences between the two fitted line segments, including the two endpoints of the fitted line segments, the intercept and slope of the corresponding line equation; the two fitted line segments found by the robot can be two adjacent fitted line segments, but not necessarily two parallel line segments; when the robot detects that the distance between the nearest endpoints of the two fitted line segments is less than a preset contour distance threshold, and the absolute value of the difference in the slopes of the two fitted line segments is less than a preset slope threshold, and the extension of the two fitted line segments... When the distance between the intersection points of the lines and the same coordinate axis (i.e., the intercept of the corresponding straight line equation in the laser coordinate system) is less than a preset intercept threshold, it is determined that the two fitted line segments are on a straight line. In some embodiments, it can be determined that the two fitted line segments represent the same obstacle or different parts of the same obstacle. Otherwise, the two fitted line segments are not on a straight line, which means that the two fitted line segments are located on two separate obstacles. Then, the robot combines the two laser points corresponding to the two fitted line segments into a new laser point group, and then uses the least squares method to fit the laser points in the merged laser point group into a new fitted line segment. The new fitted line segment can then represent an obstacle on its own.

[0044] To avoid over-segmentation of the laser point group or the aforementioned subset or point set in step A1, the robot merges, judges, and processes all the segmented point sets and their corresponding fitted line segments in step A1 to obtain several final fitted line segments; such as Figure 4 As shown, there are two adjacent line segments, and their corresponding line equations are expressed as: l m A m *x+B m *y+C m =0,l m+1 A m+1 *x+B m+1 *y+C m+1 =0, where the line l m It is composed of subset S j The fitted straight line obtained by fitting the laser point within the line, the straight line l m+1 It is composed of subset S j+1 The fitted straight line is obtained by fitting the laser points within the area. First, it is determined whether two straight line segments are close to each other. In this embodiment, d0 is used to represent the closest distance between the two straight line segments, and a preset contour distance threshold is used to represent the distance threshold at which the two straight line segments are located on the same obstacle. Figure 4 As shown, line segment l m The last point on the line segment l m+1 The distance from the first point on the line is the line l. m With line lm+1 The closest distance between them is denoted by d0. When d0 is less than a preset contour distance threshold, the straight line l is determined. m With line l m+1 If they are on the same straight line, the straight line segment l can be... m The last point on the line segment l m+1 Connecting the first points on the line forms a straight line segment, and at the same time, the subset S can be... j+1 The laser points within are incorporated into the subset S j Among the laser points within the set, the subset S that comprises the largest number of laser points is... j To achieve the subset S j The update also merges a new set of laser points to avoid over-segmentation of the subset, and accordingly, a straight line l can be fitted. m After all, the original straight segment l m With line segment l m+1 They are considered to be on the same straight line.

[0045] Preferably, for each fitted line segment, the two endpoints of the fitted line segment are respectively configured as the first and last laser points in the laser point group corresponding to the fitted line segment; each laser point group corresponds to one fitted line segment. Within the same laser point group, the distance between the first and last laser points is the maximum value among the distances between any two laser points, making the first and last laser points representative within their respective laser point groups and connected between two different laser point groups.

[0046] Preferably, in the aforementioned steps of forming a new laser point group or dividing a new subset, the robot marks the laser point that was first added to the laser point group corresponding to the fitted line segment as the first laser point in that laser point group, and marks the laser point that was last added to the laser point group corresponding to the fitted line segment as the last laser point in that laser point group. In some embodiments, to facilitate indexing the laser points within each laser point group, the earlier a laser point is added to the same laser point group, the smaller its index value, and the later a laser point is added to the same laser point group, the larger its index value. Furthermore, corresponding group index values ​​are also configured for different laser point groups, with the group index value configured for the earlier the laser point group was created or divided.

[0047] As one embodiment, in step A3, the method for performing a circle fitting operation on the fitted line segments to obtain the fitted circle includes: the robot obtaining a vector perpendicular to the fitted line segment and pointing to the origin of the laser coordinate system from each fitted line segment obtained in step A2, that is, the robot obtaining a vector perpendicular to the fitted line segment and pointing to the origin of the laser coordinate system from each fitted line segment merged in step A2 and each fitted line segment not merged in step A2 (originating from the fitted line segments corresponding to each laser point group segmented in step A112), setting the angle between this vector and the horizontal coordinate axis as the horizontal deflection angle, and setting this vector... The angle between the fitted line segment and the vertical axis is set as the vertical deflection angle. Then, the robot sets the fitted line segment as one side of an equilateral triangle, typically formed by the line connecting the first and last laser points of the laser point group corresponding to the fitted line segment. The robot then sets the circumcenter of this equilateral triangle (the center of its circumcircle) to be on either side of the fitted line segment, or on the same side as the origin of the laser coordinate system. Finally, since it is processing an equilateral triangle, the product of the length of the fitted line segment and the tangent function value of 30 degrees is set as the circumcircle of the equilateral triangle. The radius is calculated; then, the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the horizontal deflection angle is marked as a preset horizontal axis offset coordinate, and this preset horizontal axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the horizontal axis; simultaneously, the robot marks the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the vertical deflection angle as a preset vertical axis offset coordinate, and determines this preset vertical axis offset coordinate to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the vertical axis; then, using the preset vertical axis offset coordinate and... The robot calculates the coordinates of the center of the circumcircle of the equilateral triangle by using a preset horizontal axis offset coordinate. Similarly, the robot obtains the horizontal coordinate of the center of the circumcircle by adding the preset vertical axis offset coordinate to the vertical coordinate of the midpoint of the fitted line segment. After determining the coordinates of the center of the circumcircle of the equilateral triangle, the robot can determine the coverage area of ​​the circumcircle of the equilateral triangle in the laser coordinate system by combining the radius of the circumcircle.Then, based on the coordinates of the center of the circumcircle of the equilateral triangle, the robot expands the radius of the circumcircle of the equilateral triangle by a preset radius increment to obtain the preset circumcircle, and determines that the fitting line segment has completed the circle fitting operation. Each fitting line segment obtained in step A2 corresponds to a preset circumcircle; wherein, the preset radius increment is equivalent to expanding a circular boundary on the basis of the circumcircle of the equilateral triangle to ensure that obstacles are included.

[0048] Preferably, the radius of dynamic obstacles in the actual environment is not too large. In order to eliminate the influence of long corridors, walls and large obstacles, a preset allowable radius is set. Only preset circumcircles with radii smaller than the preset allowable radius can be retained.

[0049] As one embodiment, to fully represent and encompass the obstacle, all fitted line segments obtained in step A2 (considered as fitted line segments corresponding to the laser point groups processed in step A2 and fitted line segments corresponding to the laser point groups that do not require merging (derived from the fitted line segments obtained in step A13)) are fitted with circles; see [link to relevant documentation]. Figure 5 The method for performing circle fitting on a group of laser points or its corresponding fitted line segment includes: taking a straight line segment as an example, the first laser point in the group of laser points corresponding to this straight line segment is... The last laser point is This can also be considered as the two endpoints of the straight line segment; simultaneously, the robot sets the normal vector of this straight line segment pointing to the origin of the laser coordinate system as... like Figure 5 As shown, the robot uses this straight line segment as its base and designs an equilateral triangle with its circumcenter (center of the circle) far from the origin (ox, oy) of the laser coordinate system. That is, the circumcenter of this equilateral triangle and the origin of the laser coordinate system are located on opposite sides of the straight line segment. Therefore, the coverage area of ​​the circumcircle of this equilateral triangle is set further away, covering obstacles at locations farther than the center of the ranging sensor, since the obstacles are not close to the robot. Specifically, the radius of the circumcircle of this equilateral triangle is... The vector representation of the center of the circumcircle is: The principle lies in the fact that, in the aforementioned embodiment, the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the horizontal deflection angle is marked as a preset horizontal axis offset coordinate, and this preset horizontal axis offset coordinate is determined to be the coordinate offset on the horizontal axis between the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle; simultaneously, the robot marks the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the vertical deflection angle as a preset vertical axis offset coordinate, and this preset vertical axis offset coordinate is determined to be the coordinate offset on the vertical axis between the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle. Further, to ensure that the constructed circle can adequately represent the obstacle or even delineate the obstacle, the radius of the circumcircle of the equilateral triangle needs to be expanded by a preset radius increment r. enlarge , with r+r enlarge This indicates the final determined radius of the preset circumcircle, in... This indicates the coordinates of the center point of the preset circumcircle.

[0050] Based on the aforementioned embodiments, in step A3, the method for merging the preset circumscribed circles includes: when the robot detects that there are two preset circumscribed circles that are contained within each other, the preset circumscribed circle with the larger radius among the two preset circumscribed circles is retained, while the preset circumscribed circle with the smaller radius among the two preset circumscribed circles is discarded. The preset circumscribed circle with the larger radius among the two preset circumscribed circles is set as the fitting circle, so that the fitting circle marks or delineates an obstacle. This allows it to be determined which obstacle the fitting circle is used to mark, facilitating the subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle. The identification information is then marked in the robot's memory to determine which obstacle the fitting circle is used to mark, facilitating the subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle. When the robot detects the intersection of two preset circumcircles, it configures the line connecting the centers of the two preset circumcircles as a fitting line segment, and then performs a circle fitting operation on this fitting line segment to obtain a reference circumcircle. Next, it adds the radius of the reference circumcircle to the larger of the two preset circumcircles before the circle fitting operation to obtain a reference radius. If the reference radius is detected to be smaller than a preset allowable radius, it retains the center of the reference circumcircle (specifically, records the coordinates of the center), and discards the two preset circumcircles and the reference circumcircle before the circle fitting operation. Then, it sets a circle with the center of the reference circumcircle as the center and the reference radius as the radius. The circle is then shaped and marked as the fitting circle, thereby merging the two preset circumcircles before the circle fitting operation into the fitting circle. This allows a single fitting circle to mark or delineate a corresponding obstacle, and the corresponding identification information is marked in the robot's memory to determine which obstacle the fitting circle is used to mark, facilitating subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle. In summary, the different fitting circles obtained through step A3 mark or delineate the corresponding obstacles respectively, avoiding the existence of different circles (including two fitting circles with different center positions and / or two fitting circles with different radii) representing the same obstacle.

[0051] Corresponding to Figure 6 In the process of merging all preset circumcircles, the merging can be performed based on two adjacent preset circumcircles ( Figure 6 The relationship between the center and radius of the circle in the system is processed accordingly: when two preset circumcircles intersect, the centers c1 and c2 of the two preset circumcircles are connected, and the aforementioned circle fitting operation is performed on the line connecting c1 and c2 to obtain a new preset circumcircle with center c3, which is marked as the reference circumcircle. Then, the radius r3 of the reference circumcircle is added to the radius r2 of the preset circumcircle with center c2. If the radius r after the addition is less than the preset allowable radius, the coordinates c3 of the center of the reference circumcircle and the radius r are retained as the components of the fitted circle to cover the laser coordinate system.

[0052] As one embodiment, step A4 specifically includes: using the location information of obstacles represented by point cloud data, assigning corresponding pixel values ​​to each grid of the grid map, then marking grids with pixel values ​​greater than a preset pixel threshold as passable grids, and marking grids with pixel values ​​less than or equal to the preset pixel threshold as impassable grids, and then obtaining a binarized grid map, and confirming that the original grid map has undergone binarization processing; specifically, after the robot builds the grid map using the Cartographer algorithm pre-stored in the robot body, it determines the location information of obstacles represented by point cloud data, assigns corresponding pixel values ​​to each grid of the grid map, thereby obtaining the length and width information of the grid map and the pixel value of each grid, specifically marking the grid where the pixel point with a pixel value greater than 128 is located as binary 0 to form a passable grid, and conversely marking the grid where the pixel point is located as binary 1 to form an impassable grid, constructing a binary grid map composed of binary 0 and binary 1, and updating the grid map to simplify the representation of the map and the internal environment information filled. Then, the robot marks the accessible graticles in the associated neighborhood of each inaccessible graticle in the binarized grid map as inaccessible graticles according to the preset point cloud conversion error, thus obtaining the eroded grid map. The grid area of ​​the associated neighborhood of each inaccessible graticle is positively correlated with the grid area involved in the preset point cloud conversion error. Preferably, during the erosion process, the robot updates all binary 0s in the twenty-four neighborhoods centered on each graticle with a value of binary 1 to binary 1. That is, within the two adjacent rings of graticles outside each graticle with a value of binary 1, all binary 0s are updated to binary 1 to complete the erosion process of the grid map. The area covered by the twenty-four neighborhoods is a grid area with 5 rows and 5 columns, excluding the center. This area can be denoted as the effective erosion area of ​​this erosion process, or understood as an erosion radius equal to two graticles. The specific erosion radius can be adjusted according to the environment in which the robot is located in order to identify dynamic obstacles in a larger effective area. Then, the raster map, which has undergone the aforementioned binarization and erosion processes, is updated to the raster map. The area composed of passable rasters is then marked as a passable area within the raster map. At the same time, the coordinate index values ​​of the passable rasters within the raster map are recorded to form a set of passable points. In fact, the coordinate information of each raster in the global map coordinate system is saved in the form of index values. Then, step A5 is executed.

[0053] As one embodiment, in step A5, the robot projects the center of the fitted circle obtained in step A3 from the laser coordinate system onto the grid map updated in step A4. This transforms the center of all fitted circles from laser points to map index values, obtaining the coordinate index value of the corresponding fitted circle's center in the global map coordinate system of the grid map. Essentially, the coordinate information of the fitted circle's center in the global map coordinate system is stored as an index value. Preferably, the X-axis of the global map coordinate system is parallel or perpendicular to the X-axis of the laser coordinate system, and the coordinates on the same axis in both coordinate systems represent the outline size of the same obstacle at different scales; that is, the length of the obstacle outline represented by the same coordinate offset is different. Then, the coordinate index value recorded in step A4 of the aforementioned embodiment is compared with the coordinate index value of the center of the fitted circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitted circle obtained in step A3 in the global map coordinate system of the grid map is equal to one of the coordinate index values ​​recorded in step A4, it is determined that the fitted circle is a fitted pattern whose center falls into the passable area of ​​the grid map, and the robot can filter out the fitted circles whose center falls into the passable area of ​​the grid map and mark the fitted circle as the fitted pattern used to mark obstacles; when step When the coordinate index value of the center of the fitted circle obtained in A3 on the global map coordinate system of the raster map is not equal to any of the coordinate index values ​​recorded in step A4, the center of the fitted circle is removed from the raster map. Points that are not within the boundary of the raster map are removed. Specifically, the center of the circle at the boundary between the area occupied by the obstacle and the passable area is removed. The remaining points are the location information of the dynamic obstacles within the passable area of ​​the map. This is because the dynamic window can be called to detect moving obstacles and their reflected point cloud data within the passable area.

[0054] As one embodiment, in step A6, the method of obtaining the center of the fitted graphic used to mark the obstacle at different times, calculating the speed of the obstacle, and then identifying whether the obstacle is a static obstacle or a dynamic obstacle based on the speed of the obstacle includes: the robot marks the coordinates of the center of the fitted graphic used to mark the obstacle selected in step A5 at the first time (the center of the fitted circle generated at the first time under the premise of marking the same obstacle) as the first center coordinates; then, the robot marks the coordinates of the center of the fitted graphic used to mark the obstacle selected in step A5 at the second time (the center of the fitted circle generated at the second time under the premise of marking the same obstacle) as the second center coordinates; after the robot executes step A5, the basis for extracting the fitted circle used to mark the same obstacle from the coordinate index information of the grid map last updated in step A4 is the correspondence between the fitted circle and the marked obstacle, and the circle merging process in step A3 can overcome the problem of different fitted circles marking the same obstacle. The correspondence involved can be formed by the merging process of the laser point group in step A2 and the acquisition and merging process of the preset circumscribed circle in step A3 (i.e., circle merging process).

[0055] The robot marks the time difference between the first and second moments as the obstacle movement time difference. This obstacle movement time difference can be considered as the time interval between adjacent points of the same obstacle (corresponding to the center of the fitted circle in the aforementioned embodiment), and can be specifically set based on other physical factors such as the actual size, weight, and friction of the detected obstacle. Therefore, the robot marks the distance between the first and second center coordinates as the obstacle's movement distance.

[0056] Then, the ratio of the obstacle's movement distance to the obstacle's movement time difference is calculated to obtain the obstacle's speed, which is taken as the obstacle's instantaneous speed. When the robot determines that the obstacle's speed is greater than a preset speed threshold, it determines that the obstacle is a dynamic obstacle, and then repeats steps A1 to A6 to track the obstacle's motion state. This includes changes in the fitted circle used to mark the dynamic obstacle and its center, as well as changes in the laser point group and fitted line segment corresponding to the fitted circle. The preset speed threshold is configured to distinguish between static and dynamic obstacles. This speed threshold is not necessarily 0, meaning that a static obstacle may not necessarily be stationary and may move relatively slowly within the allowable error range to be considered as being stationary. Specifically, as the robot sampling time progresses, the point cloud data is refreshed, and the laser point group segmented in step A1, the fitted line segment merged in step A2, and the fitted circle obtained in step A3 are refreshed. In particular, several point cloud data reflected back by the obstacle or the fitted circle used to mark the obstacle and its center are continuously refreshed. Therefore, the least squares method can be used to obtain the fitted line equation for the multiple point cloud data (laser points) that are continuously refreshed within the time difference of the obstacle's movement. This equation is configured as the motion state equation of the obstacle, and the position information of the obstacle at the first and second moments can be determined. Specifically, under the premise of marking the same obstacle, the coordinates of the center of the fitted circle generated at the first moment and the coordinates of the center of the fitted circle generated at the second moment can be obtained. Then, the distance between these two centers can be calculated to obtain the movement distance of the obstacle, and then the movement speed of the marked same obstacle can be calculated.

[0057] In addition, when the robot determines that the speed of the obstacle is less than or equal to the preset speed threshold, it determines that the obstacle is a static obstacle, stops calculating the speed of the obstacle, and does not track the changes of the fitted circle used to mark the static obstacle and its corresponding laser point group.

[0058] In summary, by selecting a suitable fitted circle, the overall characteristics and motion information of obstacles of different state types can be calculated, which facilitates the prediction of the motion behavior of obstacles in advance and reduces the collision between the robot and dynamic obstacles.

[0059] The scope of protection of this invention is not limited to the embodiments described above. Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its scope and spirit. If these modifications and variations fall within the scope of the claims of this invention and their equivalents, then the intent of this invention also includes these modifications and variations.

Claims

1. A dynamic obstacle detection method based on fitted graphics, wherein a robot is equipped with a ranging sensor to acquire point cloud data; characterized in that, This dynamic obstacle detection method includes: The robot extracts fitted patterns from point cloud data to mark obstacles; at the same time, the robot constructs a grid map; then the robot compares the center of the fitted pattern with the grid map and selects the fitted patterns whose center falls into the passable area of ​​the grid map. Then, by using the coordinate changes of the center of the selected fitted graph, it is possible to detect whether the obstacle marked by the fitted graph is a dynamic obstacle. The dynamic obstacle detection method specifically includes: Step A1: The robot segments the acquired point cloud data to obtain several point sets; Step A2: Merge the set of points obtained in step A1 to obtain the fitted line segment; Step A3: Perform a circle fitting operation on the fitted line segment obtained in step A2 to obtain a preset circumcircle; then perform a circle merging process on the preset circumcircle to obtain the fitted circle. In step A3, the method for merging the preset circumscribed circle includes: When the robot detects that there are two preset circumcircles that are contained within each other, it retains the preset circumcircle with the larger radius and removes the preset circumcircle with the smaller radius. The preset circumcircle with the larger radius is then set as the fitting circle so that the fitting circle marks or delineates an obstacle. When the robot detects that two preset circumcircles intersect, it configures the line connecting the centers of the two preset circumcircles as a fitting line segment, and then performs the aforementioned circle fitting operation on the fitting line segment to obtain a reference circumcircle. Then, it adds the radius of the reference circumcircle to the larger radius of the two preset circumcircles before the aforementioned circle fitting operation to obtain a reference radius. When it is detected that the reference radius is less than a preset allowable radius, it retains the center of the reference circumcircle, and then sets a circle with the center of the reference circumcircle as the center and the reference radius as the radius, and marks the circle as the fitting circle, so that the fitting circle marks or delineates an obstacle. Step A4: Construct a raster map using point cloud data, then binarize the raster map based on its accessibility, then perform erosion processing on the binarized raster map, and finally update the raster map after binarization and erosion processing. Step A5: Convert the center of the fitted circle obtained in step A3 onto the map to compare it with the coordinate index information of the grid map updated in step A4, filter out the fitted circles whose centers fall into the passable area of ​​the grid map, and determine the fitted graphic obtained by the robot for marking obstacles. Step A6: By obtaining the center of the fitted graph used to mark the obstacle at different times, calculate the speed of the obstacle, and then detect whether the obstacle is a static obstacle or a dynamic obstacle based on the speed of the obstacle.

2. The dynamic obstacle detection method according to claim 1, characterized in that, In step A1, the point cloud data acquired by the robot through the ranging sensor is a laser point cloud, which includes multiple laser points. The method by which the robot segments the acquired point cloud data to obtain several point sets includes: Step A11: During the process of the robot searching for laser points, the robot uses the change in the Euclidean distance between two laser points to divide the laser point cloud into multiple laser point groups. Step A12: The robot uses the least squares method to fit a fitted line segment to the laser points in each laser point group. Within each laser point group, the robot marks the laser point farthest from the fitted line segment as the farthest point. When the robot calculates that the distance between the farthest point and the fitted line segment of the laser point group is greater than the preset segmentation distance threshold, the robot divides the laser point group into two subsets with the farthest point as the boundary so that the farthest point becomes the first laser point in one subset. Within the laser point group, the robot assigns laser points with index values ​​smaller than the farthest point to one subset and laser points with index values ​​larger than the farthest point to the other subset. Step A13: The robot updates each subset to the laser point group described in step A12, and then executes step A12 again until the distance between the farthest point in all laser point groups and the fitted line segment corresponding to the laser point group is less than or equal to the preset segmentation distance threshold. Then, laser point groups with fewer than the preset number threshold are removed. The remaining laser point groups are several point sets described in step A11, and the fitted line segments fitted by the laser points in each existing laser point group are obtained.

3. The dynamic obstacle detection method according to claim 2, characterized in that, In step A11, the method by which the robot divides the laser point cloud into multiple laser point groups using the change in the Euclidean distance between two laser points includes: Step A111: The robot calculates the Euclidean distance between the currently searched laser point and the previously searched laser point; if the Euclidean distance is less than a preset grouping distance threshold, the currently searched laser point and the previously searched laser point are grouped into the same laser point group; if the Euclidean distance is greater than or equal to the preset grouping distance threshold, the currently searched laser point is grouped into a new laser point group, and the currently searched laser point is marked as the first laser point in the new laser point group; then proceed to step A112. Step A112: The robot searches for new laser points, then updates the laser points found in the current search to the laser points found in the previous search, and then executes step A111 again until the robot has calculated the Euclidean distance between each laser point and any other laser point and divided the laser points into groups based on the Euclidean distance between the corresponding two laser points, and then executes step A12.

4. The dynamic obstacle detection method according to claim 2, characterized in that, In step A2, the method for merging the point set includes: Whenever the robot searches for two fitted line segments from all the fitted line segments obtained in step A13, if the robot detects that the distance between the nearest endpoints of the two fitted line segments is less than a preset contour distance threshold, and the absolute value of the difference in the slopes of the two fitted line segments is less than a preset slope threshold, and the distance between the intersection points of the extensions of the two fitted line segments and the same coordinate axis is less than a preset intercept threshold, then the two fitted line segments are determined to be on a straight line. Then, the two laser point groups corresponding to the two fitted line segments are merged into a new laser point group, and the least squares method is used to fit the laser points in the merged laser point group into a new fitted line segment.

5. The dynamic obstacle detection method according to claim 4, characterized in that, For each fitted line segment, the two endpoints of the fitted line segment are respectively configured as the first laser point and the last laser point in the laser point group corresponding to the fitted line segment; each laser point group corresponds to one fitted line segment; Within the same laser point group, the distance between the first and last laser point is the maximum value among the distances between any two laser points.

6. The dynamic obstacle detection method according to claim 5, characterized in that, In step A3, the method for performing circle fitting on the fitted line segment includes: The robot obtains a vector perpendicular to the fitted line segment and pointing to the origin of the laser coordinate system from each fitted line segment obtained in step A2. The angle between the vector and the horizontal axis is set as the horizontal deflection angle, and the angle between the vector and the vertical axis is set as the vertical deflection angle. Then set the fitted line segment as one side of an equilateral triangle, and set the circumcenter of the equilateral triangle to be on the same side as the origin of the laser coordinate system, either on the opposite side of the fitted line segment or on the same side as the origin of the laser coordinate system. Then, the product of the length of the fitted line segment and the tangent function value of 30 degrees is set as the radius of the circumcircle of the equilateral triangle; then, the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the horizontal deflection angle is marked as a preset horizontal axis offset coordinate, and this preset horizontal axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the horizontal axis; simultaneously, the product of half the radius of the circumcircle of the equilateral triangle and the cosine function value of the vertical deflection angle is marked as a preset vertical axis offset coordinate, and this preset vertical axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitted line segment and the center of the circumcircle of the equilateral triangle on the vertical axis; then, the coordinate position of the center of the circumcircle of the equilateral triangle is calculated using the preset vertical axis offset coordinate and the preset horizontal axis offset coordinate. Based on the coordinates of the center of the circumcircle of the equilateral triangle, the radius of the circumcircle is increased by a preset radius increment to obtain the preset circumcircle, and the circle fitting operation is completed for the fitted line segment.

7. The dynamic obstacle detection method according to claim 1, characterized in that, Step A4 specifically includes: using the location information of obstacles represented by point cloud data, assigning corresponding pixel values ​​to each grid of the grid map, then marking grids with pixel values ​​greater than a preset pixel threshold as passable grids, and marking grids with pixel values ​​less than or equal to the preset pixel threshold as impassable grids, then obtaining a binarized grid map, and confirming that the original grid map has undergone binarization processing. Then, according to the preset point cloud conversion error, the accessible rasters in the associated neighborhood of each inaccessible raster in the binarized raster map are marked as inaccessible rasters, thus obtaining the eroded raster map; wherein, the raster region of the associated neighborhood of each inaccessible raster is positively correlated with the raster region involved in the preset point cloud conversion error. Then, the raster map, which has undergone binarization and erosion processing, is updated to the raster map. The area composed of passable rasters is marked as the passable area in the raster map, and the coordinate index values ​​of the passable rasters in the raster map are recorded. Then, step A5 is executed.

8. The dynamic obstacle detection method according to claim 7, characterized in that, In step A5, the robot projects the center of the fitted circle obtained in step A3 from the laser coordinate system to the grid map updated in step A4, and obtains the coordinate index value of the center of the fitted circle in the global map coordinate system of the grid map. Then, compare the coordinate index value recorded in step A4 with the coordinate index value of the center of the fitted circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitted circle obtained in step A3 in the global map coordinate system of the grid map is equal to one of the coordinate index values ​​recorded in step A4, determine that the fitted circle is the fitted graphic whose center falls into the passable area of ​​the grid map, and determine the fitted circle whose center falls into the passable area of ​​the grid map and mark the fitted circle as the fitted graphic used to mark obstacles; when the coordinate index value of the center of the fitted circle obtained in step A3 in the global map coordinate system of the grid map is not equal to any of the coordinate index values ​​recorded in step A4, remove the center of the fitted circle from the grid map.

9. The dynamic obstacle detection method according to claim 8, characterized in that, In step A6, the method of calculating the speed of the obstacle by obtaining the center of the fitted graph used to mark the obstacle at different times, and then identifying whether the obstacle is a static obstacle or a dynamic obstacle based on the speed of the obstacle includes: The robot marks the coordinates of the center of the fitted graph selected in step A5 at the first time as the first center coordinates; then, the robot marks the coordinates of the center of the same fitted graph selected in step A5 at the second time as the second center coordinates. The robot marks the time difference between the first and second moments as the obstacle movement time difference; The robot marks the distance between the first center coordinate and the second center coordinate as the distance traveled by the obstacle; Then, calculate the ratio of the obstacle's travel distance to the time difference of its travel to obtain the obstacle's speed; When the robot determines that the speed of the obstacle is greater than a preset speed threshold, it determines that the obstacle is a dynamic obstacle; When the robot determines that the speed of the obstacle is less than or equal to a preset speed threshold, it determines that the obstacle is a static obstacle.

Images