A robot obstacle avoidance path planning method

By using the ranging sensor to obtain point cloud data and build a grid map, combined with the DWA algorithm and heuristic search algorithm, the robot can effectively avoid dynamic obstacles, solving the problem that sweeping robots are difficult to avoid dynamic obstacles during full coverage cleaning. , improves cleaning efficiency.

CN115268443BActive Publication Date: 2024-11-29AMICRO SEMICONDUCTOR CO LTD +1

Patent Information

Application Number
CN202210893735.6
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-07-27
Publication Date
2024-11-29
Estimated Expiration
2042-07-27

AI Technical Summary

Technical Problem

It is difficult for the sweeping robot to effectively avoid dynamic obstacles during full coverage cleaning, resulting in frequent collisions.

Method used

Point cloud data is obtained by assembling the ranging sensor, fitted graphics marking obstacles, and raster maps are constructed. Based on DWA algorithm and heuristic search algorithm, we plan obstacle avoidance paths and predict the movement behavior of obstacles in advance.

Benefits of technology

The robot effectively avoids obstacles in a dynamic obstacle environment, reduces the number of collisions with obstacles, and improves the cleaning efficiency.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115268443B_ABST
    Figure CN115268443B_ABST
Patent Text Reader

Abstract

The invention discloses a robot obstacle avoidance path planning method, wherein the robot is equipped with a distance measuring sensor for acquiring point cloud data; the robot obstacle avoidance path planning method comprises the following steps: step A, the robot extracts a fitting graphic for marking obstacles from the point cloud data; at the same time, the robot constructs a grid map; then the robot compares the center of the fitting graphic with the grid map, and screens out the fitting graphic whose center falls into the passable area of ​​the grid map; step B, based on the mapping situation of the currently screened fitting graphic, calling an optimized DWA algorithm to plan an obstacle avoidance path; or when calling the optimized DWA algorithm to plan the obstacle avoidance path fails, calling a heuristic search algorithm to plan the obstacle avoidance path, so that the robot avoids the obstacle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the field of mobile robot path planning and obstacle avoidance, and in particular relates to a robot obstacle avoidance path planning method. Background Art

[0002] In recent years, with the continuous improvement of people's living standards, sweeping robots as a service-oriented mobile robot have been widely introduced into various families, providing convenience for people's daily lives. Path planning technology is one of the key technologies of sweeping robots, and the performance of its algorithm directly affects the working efficiency of sweeping robots. Path planning of sweeping robots is an important research field in robotics. From the perspective of tasks, it can be divided into two categories: point-to-point planning and full-coverage path planning. Point-to-point planning focuses on how to reach the end point with the optimal path, and full-coverage planning requires the robot to completely traverse a certain plane area or space range.

[0003] In an indoor home environment, a sweeping robot is prone to encounter dynamic obstacles such as people and pets when performing full-coverage cleaning. Those skilled in the art will classify obstacles into dynamic obstacles and static obstacles according to their state types. When a sweeping robot avoids dynamic obstacles in the environment, it treats the dynamic obstacles as instantaneous static obstacles or fails to detect the dynamic obstacles. At the same time, due to the measurement accuracy of the laser, it is easy to disperse the obstacles into scattered points, resulting in the overall characteristics of the obstacles being ignored. In extreme cases, a path through the obstacles may be planned. There is a lack of strategies to guide the sweeping robot to avoid dynamic objects in the indoor environment, and the movement behavior of obstacles cannot be predicted in advance, resulting in frequent collisions between the sweeping robot and dynamic obstacles during full-coverage cleaning or partial cleaning. Summary of the invention

[0004] In view of the problem that the robot lacks a dynamic obstacle avoidance strategy during movement, the present invention discloses a robot obstacle avoidance path planning method, and the specific technical solution is as follows:

[0005] A robot obstacle avoidance path planning method, the robot is equipped with a ranging sensor for acquiring point cloud data; the robot obstacle avoidance path planning method comprises the following steps: step A, the robot extracts a fitting graphic for marking obstacles from the point cloud data; at the same time, the robot constructs a grid map; then the robot compares the center of the fitting graphic with the grid map, and screens out the fitting graphic whose center falls into the passable area of ​​the grid map; step B, based on the mapping situation of the currently screened fitting graphic, calling an optimized DWA algorithm to plan an obstacle avoidance path; or when calling the optimized DWA algorithm to plan the obstacle avoidance path fails, calling a heuristic search algorithm to plan the obstacle avoidance path.

[0006] Furthermore, the step B specifically includes the following steps: step B1, after the robot screens out the corresponding fitting graphic by executing step A, the evaluation function of the corresponding dimension in the DWA algorithm is introduced and the corresponding weighting parameter is adjusted, and then the obstacle avoidance path is planned in combination with the mapping of the currently screened fitting graphic in the sampling window of the DWA algorithm, so as to call the optimized DWA algorithm to plan the obstacle avoidance path; step B2, when the robot fails to plan the obstacle avoidance path using the optimized DWA algorithm, when the robot detects an obstacle, the robot plans the obstacle avoidance path based on the mapping of the fitting graphic screened out in step A in the grid map using a heuristic search algorithm.

[0007] Furthermore, in the step B, the robot sets a sampling window, the size of which is set to limit the vector speed of the robot, and the sampling window is set to cover the entire outline of the obstacle when the robot detects an obstacle; the robot calls the optimized DWA algorithm in the following manner: the robot uses the sampling window to sample the vector speed of the robot; then, the robot normalizes the evaluation functions of each dimension pre-set in the DWA algorithm, and then ratios the normalized results of the evaluation functions of the two selected dimensions to obtain the size relationship between the motion cost weighted parameters; then, the size relationship between the motion cost weighted parameters is adjusted according to the distance relationship between the robot and the obstacle marked by the currently screened fitting graphic, and then the sampling data of the sampling window is scored in combination with the size relationship between the adjusted motion cost weighted parameters to obtain the optimal motion trajectory, which corresponds to the obstacle avoidance path, so that the robot does not collide with the obstacle when walking along the optimal motion trajectory.

[0008] Furthermore, in the step B, it also includes: the robot controls the fitting graph screened out in step A to expand and discretize into multiple points, obtains mapping information of the obstacle marked by the fitting graph within the sampling window, so as to form a cost map of the obstacle in its speed direction; then extracts the distance between the robot and the obstacle marked by the fitting graph screened out in step A in real time from the sampling information, so that the path planned by the DWA algorithm on the cost map avoids obstacles in motion; wherein, the greater the speed of the obstacle marked by the fitting graph, the greater the coverage formed by the fitting graph after the expansion processing; when the vector speed of the robot includes the linear speed of the robot and its angular speed, after scoring the sampling data of the sampling window in combination with the size relationship between the adjusted motion cost weighting parameters, the robot selects the speed combination with the highest score to plan a motion trajectory suitable for the robot to move according to the speed combination.

[0009] Furthermore, in step B1, it also includes: within the preset sampling time, when the robot moves at the currently set speed, if the distance between the robot and the center of the fitting figure screened out in step A is a preset safety distance, or there is a fitting figure screened out in step A on one side of the robot but the robot does not collide with the obstacle marked by the fitting figure on that side, then stop calling the optimized DWA algorithm; when the distance between the current position of the robot and the center of the fitting figure used to mark the obstacle is less than or equal to the preset safety distance, if the robot does not change the current forward direction or maintains the current forward direction until it collides with an obstacle, it is determined that the robot has failed to plan the obstacle avoidance path using the optimized DWA algorithm, and then stop calling the optimized DWA algorithm.

[0010] Furthermore, in the optimized DWA algorithm, the robot normalizes the evaluation functions of various dimensions pre-set in the DWA algorithm, and then ratios the normalized results of the evaluation functions of the selected two dimensions, and the method for obtaining the size relationship between the motion cost weighted parameters includes: the evaluation functions of various dimensions set by the robot are respectively an azimuth evaluation function, a distance evaluation function and a robot speed evaluation function; the motion cost weighted parameter applied by the robot to the azimuth evaluation function is the azimuth weight; the motion cost weighted parameter applied by the robot to the distance evaluation function is the distance weight; the motion cost weighted parameter applied by the robot to the robot speed evaluation function is the robot speed weight; for the evaluation function of each dimension, the normalization method includes: after the robot uses the sampling window to sample the robot's vector velocity, Based on the data, a predicted trajectory is planned according to the kinematic model; in the process of traversing each node of the predicted trajectory, the robot accumulates the function value generated by the evaluation function at each node of the predicted trajectory, obtains the statistical sum value of the evaluation function, and obtains the maximum evaluation function value and the minimum evaluation function value generated by the evaluation function; then the ratio of the absolute value of the difference between the maximum evaluation function value and the minimum evaluation function value to the statistical sum value of the evaluation function is marked as the normalized result of the evaluation function; within the allowable error range of the motion cost weighting parameter, the robot configures the product of the azimuth weight and the normalized result of the azimuth evaluation function to be equal to the product of the distance weight and the normalized result of the distance evaluation function; the robot also configures the product of the distance weight and the normalized result of the distance evaluation function to be equal to the product of the robot speed weight and the normalized result of the robot speed evaluation function.

[0011] Further, in the optimized DWA algorithm, the size relationship between the weighted parameters of the motion cost is adjusted according to the distance relationship between the robot and the detected obstacle, including: when the robot detects that the center of the fitting figure screened out in step A does not exist in the circular area with itself as the center and a radius of the preset safety distance, the robot adjusts both the azimuth angle weight and the distance weight to be greater than the robot speed weight, so that the robot accelerates to move toward the preset target point; when the robot detects that the minimum distance between the predicted trajectory and the center of the fitting figure screened out in step A is less than the preset safety distance, the robot adjusts both the distance weight and the robot speed weight to be greater than the azimuth angle weight, so that the robot does not collide with the obstacle marked by the fitting figure screened out in step A during the turning process; when the robot is located at the previous path node of the path end point of the predicted trajectory, the robot adjusts the azimuth angle weight to be greater than both the distance weight and the robot speed weight, so that the robot decelerates to move to the path end point; wherein the score of each predicted trajectory is configured to be equal to the result of the weighted average of the evaluation function of the corresponding dimension.

[0012] Furthermore, after the robot fails to plan an obstacle avoidance path using the optimized DWA algorithm, when the robot does not touch the obstacle, if there is point cloud data reflected from the obstacle in the sampling window, the robot uses the coordinates of the center of the fitting figure screened out in step A as the mean and the parameter positively correlated with the speed of the obstacle marked by the fitting figure as the standard deviation to establish a two-dimensional Gaussian distribution, and then uses the two-dimensional Gaussian distribution to discretize the edge line of the fitting figure into multiple points, and then maps these discrete points to the grid map and marks them as grids occupied by obstacles, and then uses the heuristic search algorithm to plan a path on the grid map to bypass the grids occupied by obstacles, and determines to use the heuristic search algorithm to plan the obstacle avoidance path.

[0013] Furthermore, the method of using a heuristic search algorithm to plan a path that bypasses a grid occupied by an obstacle on the grid map includes: the robot first sets a path starting point and a path end point on a global map coordinate system; then converts the coordinates of a current position and / or a preset target point to a global map coordinate system, and then uses a heuristic search algorithm to plan a path that bypasses a grid occupied by an obstacle, and obtains multiple path nodes and path extension direction information located in the global map coordinate system, so as to realize the use of a heuristic search algorithm to plan an obstacle avoidance path, wherein the path starting point and the path end point are respectively the starting point and the end point of the obstacle avoidance path, and the current position of the robot or the preset target point is the search starting point specified by the heuristic search algorithm.

[0014] Furthermore, step A also includes: using the coordinate change of the center of the screened fitting graph to identify the state type of the obstacle marked by the fitting graph to distinguish between static obstacles and dynamic obstacles.

[0015] Furthermore, the step A specifically includes: step A1, the robot segments the acquired point cloud data to obtain a plurality of point sets; step A2, the point sets obtained in step A1 are merged to obtain fitting line segments; step A3, the fitting line segments obtained in step A2 are subjected to circle fitting operation to obtain a preset circumscribed circle; the preset circumscribed circle is then subjected to circle merging operation to obtain a fitting circle; step A4, a grid map is constructed using the point cloud data, the grid map is binarized according to the accessibility, the binarized grid map is corroded, and the binarized grid map is converted into a grid map. The grid map after the processing and corrosion is updated to the grid map; step A5, the center of the fitting circle obtained in step A3 is converted to the map to achieve comparison with the coordinate index information of the grid map finally updated in step A4, the fitting circle whose center falls into the passable area of ​​the grid map is screened out, and it is determined that the robot obtains the fitting figure for marking the obstacle; step A6, by obtaining the center of the fitting figure for marking the obstacle at different times, the speed of the obstacle is calculated, and then the obstacle is identified as a static obstacle or a dynamic obstacle according to the speed of the obstacle.

[0016] Furthermore, 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 multiple laser points; the method for the robot to segment the acquired point cloud data to obtain a plurality of point sets includes: step A11, in the process of the robot searching for laser points, the robot uses the change of 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 fitting line segment for the laser points in each laser point group; in each laser point group, the robot marks the laser point farthest from the corresponding fitted fitting line segment as the farthest point, and when the robot calculates that the distance between the farthest point and the corresponding fitted fitting line segment of the laser point group is greater than a preset segmentation distance threshold, the farthest point is used as the boundary to segment the laser point group. The laser point group is divided into two subsets so that the farthest point becomes the first laser point in one of the subsets, wherein, in the laser point group, the robot classifies the laser points with smaller index values ​​than the farthest point into one subset, and classifies the laser points with larger index values ​​than the farthest point into another subset; step A13, the robot updates each subset to the laser point group described in step A12, and then executes step A12 until the distance between the corresponding 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, and then the laser point groups with the number of laser points less than the preset number threshold are eliminated, and 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.

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

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

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

[0020] Further, in step A3, the method of performing a circle fitting operation on the fitting line segment includes: the robot obtains a vector perpendicular to the fitting line segment and pointing to the origin of the laser coordinate system from each fitting line segment obtained in step A2, sets the angle between the vector and the horizontal coordinate axis as the horizontal deflection angle, and sets the angle between the vector and the vertical coordinate axis as the vertical deflection angle; then sets the fitting line segment as a side of an equilateral triangle, and then sets the circumcenter of the equilateral triangle to be separated from the origin of the laser coordinate system on both sides of the fitting line segment, or to be located on the same side of the fitting line segment as the origin of the laser coordinate system; then sets the product of the length of the fitting line segment and the tangent function value of 30 degrees as the radius of the circumscribed circle of the equilateral triangle; then multiplies half of the radius of the circumscribed circle of the equilateral triangle by the cosine function of the horizontal deflection angle. The product of the values ​​is marked as a preset horizontal axis offset coordinate, and the preset horizontal axis offset coordinate is determined to be the coordinate offset between the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the horizontal axis; at the same time, the product of half the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the vertical deflection angle is marked as a preset vertical axis offset coordinate, and the preset vertical axis offset coordinate is determined to be the coordinate offset between the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the vertical axis; then the coordinate position of the center of the circumscribed circle of the equilateral triangle is calculated using the preset vertical axis offset coordinate and the preset horizontal axis offset coordinate; on the basis of the coordinate position of the center of the circumscribed circle of the equilateral triangle, the radius of the circumscribed circle of the equilateral triangle is enlarged by a preset radius increment to obtain the preset circumscribed circle, and it is determined that the circle fitting operation is completed for the fitting line segment.

[0021] Further, in step A3, the method for performing circle merging processing on the preset circumscribed circles includes: when the robot detects that there are two preset circumscribed circles in an inclusion relationship, retaining the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles, while eliminating the preset circumscribed circle with a relatively small radius among the two preset circumscribed circles, and setting the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles as a fitting circle, so that the fitting circle marks or encircles an obstacle; when the robot detects that there are two preset circumscribed circles intersecting, configuring the line connecting the centers of the two preset circumscribed circles as a fitting line segment, and then performing the aforementioned circle fitting operation on the fitting line segment to obtain a reference circumscribed circle; then adding the radius of the reference circumscribed circle to the relatively large radius of the two preset circumscribed circles before the aforementioned circle fitting operation to obtain a reference radius; when it is detected that the reference radius is smaller than the preset allowable radius, retaining the center of the reference circumscribed circle, and then setting a circle with the center of the reference circumscribed circle as the center and the reference radius as the radius, and marking the circle as the fitting circle, so that the fitting circle marks or encircles an obstacle.

[0022] Further, the step A4 specifically includes: using the position information of the obstacle represented by the point cloud data to assign corresponding pixel values ​​to each grid of the grid map, then marking the grid with a pixel value greater than a preset pixel threshold as a passable grid, and marking the grid with a pixel value less than or equal to the preset pixel threshold as an inaccessible grid, and then obtaining a binarized grid map, and determining that the original grid map has been binarized; then marking the passable grids in the associated neighborhood of each inaccessible grid in the binarized grid map as inaccessible grids according to the preset point cloud conversion error, and obtaining the grid map after corrosion processing; wherein the grid area of ​​the associated neighborhood of each inaccessible grid is positively correlated with the grid area involved in the preset point cloud conversion error; then updating the grid map that has been binarized and eroded to the grid map, and then marking the area composed of passable grids as a passable area in the grid map, and recording the coordinate index value of the passable grid in the grid map; and then executing step A5.

[0023] Furthermore, in step A5, the robot projects the center of the fitting circle obtained in step A3 from the laser coordinate system to the grid map last updated in step A4, and obtains the coordinate index value of the center of the corresponding fitting circle in the global map coordinate system of the grid map; then compares the coordinate index value recorded in step A4 with the coordinate index value of the center of the fitting circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitting 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 fitting circle is the fitting figure whose center falls into the passable area of ​​the grid map screened out in step A, then the fitting circle whose center falls into the passable area of ​​the grid map is screened out and marked as the fitting figure for marking obstacles; when the coordinate index value of the center of the fitting circle obtained in step A3 in the global map coordinate system of the grid map is not equal to any one of the coordinate index values ​​recorded in step A4, the center of the fitting circle is removed from the grid map.

[0024] Further, in the step A6, the method of obtaining the center of the fitting figure for marking 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 according to the speed of the obstacle includes: the robot marks the coordinates of the center of the fitting figure screened out in step A5 at the first time as the first center coordinates; then, the robot marks the coordinates of the center of the same fitting figure screened out 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 coordinate and the second center coordinate as the movement distance of the obstacle; 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, it determines that the obstacle is a dynamic obstacle, and then repeats steps A1 to A6 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, it determines that the obstacle is a static obstacle.

[0025] The beneficial technical effect of the present invention is that: the present invention calculates the weight coefficient in combination with the normalized result of the evaluation function, and adaptively adjusts the weight coefficient of the evaluation function introduced in the various positional relationships between the robot and the obstacle, and the predicted trajectory (derived from the trajectory corresponding to the kinematic model inference) and the obstacle (marked or circled by the fitting graph), changes the score of the pre-planned trajectory, forms the optimal trajectory, and enables the optimized DWA algorithm to select a planning path with a higher score that effectively avoids obstacles, predicts the movement behavior of the obstacle in advance, and reduces the number of collisions between the robot and the dynamic obstacle. The dynamic obstacle information is also used to generate a two-dimensional Gaussian distribution and map it to a binary grid map. Then, when the optimized DWA algorithm fails to execute (generally when the robot collides with an obstacle), the A* algorithm can be used on the map to plan a path to avoid the dynamic obstacle and determine the path to be planned to bypass the grid occupied by the obstacle using the heuristic search algorithm, so as to achieve the effect of dynamic obstacle avoidance of the robot under the condition of global path planning. In addition, the optimized DWA algorithm or A* algorithm can be selectively called to reduce the operating load of the robot during navigation operations and avoid the robot's operating memory being in a high-load state all the time. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is a flow chart of a robot obstacle avoidance path planning method disclosed in an embodiment of the present invention.

[0027] Figure 2 It is a schematic diagram of dividing a laser point cloud into two laser point groups disclosed in an embodiment of the present invention.

[0028] Figure 3 It is a schematic diagram of segmenting a laser point group disclosed in an embodiment of the present invention.

[0029] Figure 4 It is a schematic diagram of merging two fitting line segments disclosed in an embodiment of the present invention.

[0030] Figure 5 It is a schematic diagram of a circumscribed circle of an equilateral triangle disclosed in an embodiment of the present invention.

[0031] Figure 6 It is a schematic diagram of merging two preset circumscribed circles into a fitting circle disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0032] In order to facilitate those skilled in the art to understand and implement the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the implementation examples described herein are only used to illustrate and explain the present invention and are not intended to limit the present invention. The technical scheme of the present invention is further specifically described below through embodiments and in conjunction with the accompanying drawings.

[0033] The present invention discloses a robot obstacle avoidance path planning method applicable to obstacle environments of various states and types. The execution subject of the robot obstacle avoidance path planning method is an automatically moving robot, and a ranging sensor is installed on the body of the robot for obtaining point cloud data. These point cloud data are the result of the modulated light signal emitted by the ranging sensor scanning the environment in which the robot is located, specifically the information reflected by the obstacles in the environment in which the robot is located, reflecting the position information of the entire outline or partial outline of the obstacle detected by the robot. Preferably, the ranging sensor is a laser radar that supports 360-degree rotation, and obtains laser point clouds in various angles and directions, that is, the point cloud data, which consists of rotation angle information and distance information; generally, each time the laser radar rotates one circle, a frame of laser point cloud is obtained. The robot obstacle avoidance path planning method comprises: step A, the robot extracts a fitting figure for marking an obstacle from the point cloud data, that is, constructs a figure that can represent the obstacle in the point cloud data, preferably a circle that surrounds (or encircles) the obstacle. At the same time, the robot constructs a grid map, which can be simplified into a binary grid map composed of grids represented by 0 and 1, or a probabilistic grid map; wherein the fitting graphics used to mark obstacles are not necessarily located in the grid map. Therefore, the robot needs to compare the center of the fitting graphics with the grid map, and filter out the fitting graphics whose centers fall into the passable area of ​​the grid map. Specifically, the robot will first convert the center of the fitting graphics to the grid map, and use image processing to eliminate the fitting graphics whose centers are on the map boundary, thereby filtering out the fitting graphics whose centers fall into the passable area of ​​the grid map, and obtaining the location information of possible dynamic obstacles inside the grid map. Then execute step B.

[0034] Step B: After the robot screens out the corresponding fitting graphics by executing step A, based on the mapping of the currently screened fitting graphics, including the projection of the currently screened fitting graphics in the grid map described in step A, the optimized DWA algorithm is called to enable the robot to avoid obstacles, including dynamic obstacles; or when calling the optimized DWA algorithm to plan the obstacle avoidance path fails, the heuristic search algorithm is called to plan the obstacle avoidance path.

[0035] It should be noted that when the robot screens out the corresponding fitting graph by executing step A, the robot has detected the obstacle, which can be understood as the robot has identified the state type of the obstacle, which can be a dynamic obstacle, or only the entire outline or partial outline of the obstacle is captured in the corresponding search space or sampling window. The obstacle avoidance path is a path configured to avoid obstacles, and follows the relevant constraints of the optimized DWA algorithm or heuristic search algorithm, including the number of path nodes, path cost, movement direction and other related constraints, which can be the fitting results of point cloud data or the grid map mapped under some probability distribution models; regardless of whether the robot collides with an obstacle when currently planning the path, the obstacle avoidance path planned by the robot allows the robot to avoid obstacles (including dynamic obstacles) in a timely manner, providing effective protection for the robot's collision-free movement in the environment of dynamic obstacles or static obstacles.

[0036] On the basis of the above-mentioned embodiment, a motion controller is used to make the robot follow the planned obstacle avoidance path, that is, the planned obstacle avoidance path is input and the speed controlled by the robot is output; for a wheeled differential robot, the linear speed and angular speed controlled by the robot are output, so as to achieve the purpose of the robot following the obstacle avoidance path in real time. Since the obstacle avoidance path is to avoid obstacles, the robot will naturally not collide with obstacles when walking along the obstacle avoidance path.

[0037] As an embodiment, the step B specifically includes the following steps:

[0038] Step B1: On the premise that there is no collision with an obstacle, it can be when the robot detects an obstacle, that is, when the entire outline or partial outline of the obstacle is captured in the corresponding search space or sampling window, the robot selects the corresponding fitting graph by executing step A, introduces the evaluation function of the corresponding dimension in the DWA algorithm and adjusts the corresponding weighting parameter, and then plans the obstacle avoidance path in combination with the mapping of the currently selected fitting graph in the sampling window of the DWA algorithm, so as to realize calling the optimized DWA algorithm to plan the obstacle avoidance path. Preferably, when the robot touches the obstacle, it is determined that the robot fails to plan the obstacle avoidance path using the optimized DWA algorithm. Among them, the mapping of the fitting graph in the sampling window of the DWA algorithm, in order to allow the robot to have more reaction time when encountering dynamic obstacles during navigation, is adaptively expanded by using the change speed, center location and coverage area size of the fitting graph, so as to discretize the edge of the fitting graph into a number of points as the position information of the obstacle marked by the fitting graph, and add it as a part of the sampling information in the sampling window of the DWA algorithm.

[0039] Step B2: When the robot fails to plan the obstacle avoidance path using the optimized DWA algorithm (equivalent to the DWA algorithm also failing), when the robot detects an obstacle, that is, when the entire outline of the obstacle is captured in the sampling window (the sampling window frames all the point cloud data reflected by the obstacle to cover the entire outline of the obstacle), the robot plans the obstacle avoidance path using the heuristic search algorithm based on the mapping of the fitting graph screened out in step A in the grid map. Specifically, after determining the projection area of ​​the fitting graph in the grid map, the heuristic search algorithm is used in the grid map to plan the obstacle avoidance path, guide the robot to avoid the obstacle, and achieve local obstacle avoidance. When the obstacle is identified as a dynamic obstacle, a path to avoid the dynamic obstacle is planned to achieve dynamic obstacle avoidance. As for the mapping of the fitting graph in the grid map, specifically, the coordinates of the center of the fitting graph are converted to the corresponding position on the cost map, and are discretized into several points and mapped to the grid map; and the cost value of the grid around the obstacle can be updated using the Gaussian distribution. Preferably, the cost map can start from the center of the fitting graph, and the cost value of the grid along the direction of the movement speed of the obstacle becomes larger.

[0040] As an embodiment, in step B, the robot sets a sampling window, the size of the sampling window is set to limit the vector speed of the robot, and the sampling window is set to cover the entire outline of the obstacle when the robot detects the obstacle, that is, the obstacle blocks the sampling window as a whole, which is particularly commonly used in the DWA algorithm. It should be noted that the DWA algorithm (dynamic window approach) is mainly based on the principle of sampling multiple groups of speeds in the speed space (linear speed and angular speed), simulating the motion trajectories of these speeds within a certain period of time, and evaluating these trajectories through an evaluation function, and selecting the (linear speed and angular speed) corresponding to the optimal trajectory to drive the robot to move. The robot's motion state, including its constantly changing position and motion direction, is actually determined by its current motion speed and angular velocity, and is also subject to the adjustment of evaluation functions of various dimensions; the core part of the DWA algorithm is the dynamic window, and the velocity space (linear velocity and angular velocity) is created in the form of a window, that is, a rectangular box used to frame part of the velocity data, which is used as a limited range. Within this limited range, the position that can be reached at each speed and angular velocity is calculated to simulate the motion trajectory of these speeds within a certain period of time; each position is evaluated (the evaluation content includes the distance from the obstacle, the angle toward the end point, etc.), and the best position is selected, and then the above process is repeated from this best position to establish a new window, so that the window moves, that is, the dynamic window, which is described as a sampling window in this embodiment.

[0041] The method for the robot to call the optimized DWA algorithm includes: the robot uses a sampling window to sample the vector velocity of the robot; then, the robot normalizes the evaluation functions of various dimensions pre-set in the DWA algorithm to convert the weighted parameters to be in the same numerical range as the occupancy probability value of each grid in the grid map; then, the normalized processing results of the evaluation functions of the selected two dimensions are compared to obtain the size relationship between the motion cost weighted parameters, specifically, the ratio between the motion cost weighted parameters of the selected two dimensions; then, according to the distance relationship between the robot and the obstacle marked by the currently selected fitting graphic (which can be regarded as the distance relationship between the center of the fitting graphic selected in step A and the current position of the robot (the body center of the robot)), the size relationship between the motion cost weighted parameters is adjusted, and then the sampling data of the sampling window is scored in combination with the size relationship between the adjusted motion cost weighted parameters to obtain the optimal motion trajectory, which corresponds to the obstacle avoidance path, so that the robot does not collide with obstacles when walking along the optimal motion trajectory.

[0042] Preferably, in the step B, it also includes: the fitting graph screened out in the robot control step A is expanded to be discretized into multiple points, and the mapping information of the obstacle marked by the fitting graph in the sampling window is obtained. Specifically, the edge line of the fitting graph covered by the sampling window is converted into a discrete point, and the points on the edge line of the fitting graph can be extracted as discrete points at a certain angle interval. The distance between the discrete point and the center of the fitting graph can be increased or kept unchanged, and the points in the neighborhood of the extracted point on the edge line are eliminated or the pixel value changes greatly to distinguish. When the fitting graph or its discrete points need to be converted to the map coordinate system, a cost map of the obstacle in its speed direction can be formed; and then the distance between the robot and the detected obstacle is extracted in real time from the sampling information, so that the path planned by the DWA algorithm on the cost map is to avoid the obstacle in motion; wherein, the greater the speed of the obstacle marked by the fitting graph, the larger the coverage area formed by the fitting graph after the expansion processing, and when the fitting graph is a circle, the larger the radius of the circle, the larger the coverage area of ​​the fitting graph, and vice versa. Based on the path planning method of the DWA algorithm, when the vector velocity of the robot includes the linear velocity and angular velocity of the robot, after scoring the sampling data of the sampling window based on the size relationship between the adjusted motion cost weighted parameters, the robot selects the speed combination with the highest score to plan a motion trajectory suitable for the robot to move according to the speed combination, thereby forming the obstacle avoidance path.

[0043] It should be noted that, in the process of planning the path using the DWA algorithm, the robot samples the speed (including the robot's linear speed and angular speed) according to the robot speed limit, acceleration limit, and obstacle distance limit, and designs an evaluation function to score each set of sampled data or each type of speed, and finally selects the speed combination with the highest score, wherein is a set of all vector speeds that the robot can reach, and the robot will be affected by the maximum linear speed, the minimum linear speed, the maximum angular speed, and the minimum angular speed. In some embodiments, the acceleration has a range limit, so only the speed that can be reached by the maximum acceleration or the maximum deceleration within a certain time will be retained; in order to stop before encountering an obstacle, it is necessary to determine the maximum deceleration and the closest distance to the obstacle on the trajectory; this embodiment specifically adjusts the weighted parameters of the evaluation functions of each dimension and normalizes the evaluation functions of each dimension to convert the weighted parameters to the same numerical range as the occupancy probability value of each grid in the grid map, and implements the optimized DWA algorithm so that the weighted average result of the evaluation function of each dimension reaches the preset index, and the motion trajectory planned under this preset index can more flexibly deal with the collision problem of dynamic obstacles.

[0044] In the DWA algorithm or the optimized DWA algorithm, the linear velocity and angular velocity sampled by the sampling window are used to simulate the trajectory of the robot's movement. The robot's kinematic model must be analyzed first. The robot's kinematic model involves the velocity in the horizontal axis direction, the velocity in the vertical axis direction, and the rotation angle in the robot coordinate system to form a position and velocity relationship at two different times. Specifically, as the linear acceleration and angular acceleration of the motor change, the size of the dynamic window is also adaptively adjusted. Among the speeds and covered positions sampled by the dynamic window, the robot is subject to the constraints of the maximum and minimum linear and angular velocities, the maximum acceleration or the maximum deceleration that can be reached within a certain period of time, and stops before the obstacle at the maximum deceleration within the closest distance to the obstacle on the corresponding trajectory without colliding with the obstacle. Therefore, when using a dynamic window for speed sampling, the corresponding trajectory is inferred according to the aforementioned robot kinematic model (the sampling trajectory is obtained), and then the evaluation function is introduced to score the trajectory, and the trajectory with the highest score is selected. Under the premise that the relevant weight parameters of the evaluation function are not adjusted, the obstacle avoidance path planned by the DWA algorithm is formed; under the premise that the relevant weight parameters of the evaluation function are adjusted, the obstacle avoidance path planned by the optimized DWA algorithm is formed. It should be noted that the optimal motion trajectory is the predicted trajectory with the highest score among the results of the weighted average of the adjusted motion cost weighting parameters and the evaluation function of the corresponding dimension.

[0045] Preferably, in step B1, it also includes: within the preset sampling time, when the robot moves at the currently set speed, if the distance between the robot and the obstacle marked by the fitting graphic selected in step A is the preset safety distance, or there is an obstacle marked by the fitting graphic selected in step A on one side of the robot but the robot does not collide with the obstacle on this side (does not affect the normal movement of the robot to bypass the obstacle marked by the fitting graphic selected in step A without hindrance), or the distance between the robot and the center of the fitting graphic selected in step A is the preset safety distance, or there is a fitting graphic selected in step A on one side of the robot but the robot does not collide with the obstacle marked by the fitting graphic on this side, then stop calling the optimized DWA algorithm. In this way, the optimized DWA algorithm can be selectively called, and the running load of the robot during navigation operation can be reduced, so as to avoid the running memory of the robot being in a high load state all the time, or because the optimized DWA algorithm plays a role in path planning and needs to sample too much speed information, resulting in more speed waste.

[0046] It should be noted that when the robot walks along the obstacle avoidance path planned by the optimized DWA algorithm, if the robot obtains the entire outline of the obstacle in front within the built-in sampling window, and the distance between the robot's current position and the obstacle (the center of the fitting figure used to mark the obstacle) is less than or equal to the body radius, although the robot does not touch the obstacle, it does not change its direction of movement, then it is determined that the optimized DWA algorithm has failed to plan the obstacle avoidance path or the robot has failed to call the optimized DWA algorithm. At the same time, the robot continues to move straight at the current speed until it collides with the obstacle. At this time, it can be more accurately determined that the robot has failed to use the optimized DWA algorithm to plan the obstacle avoidance path. Alternatively, in step B1, it is assumed that the robot has acquired the entire outline of the obstacle ahead within a built-in sampling window. When the distance between the current position of the robot and the center of the fitting figure used to mark the obstacle is less than or equal to a preset safety distance, if the robot does not change its current direction of travel or maintains its current direction of travel until it collides with an obstacle, it is determined that the robot has failed to plan an obstacle avoidance path using the optimized DWA algorithm, and then the optimized DWA algorithm is stopped from being called; wherein, on the premise that the robot has not changed its current direction of travel, it is determined that the robot has failed to plan an obstacle avoidance path using the optimized DWA algorithm, and then the optimized DWA algorithm is stopped from being called, and the obstacle avoidance path planned by the heuristic search algorithm is promptly called instead, so that the robot can successfully avoid the obstacle before colliding with the obstacle.

[0047] Preferably, the preset safety distance is the body radius of the robot, and the preset sampling time is associated with the acceleration of the robot (which can be understood as deceleration or braking speed). When the acceleration of the robot is configured to be larger, the preset sampling time is configured to be smaller; when the acceleration of the robot is configured to be smaller, the preset sampling time is configured to be larger.

[0048] As an embodiment, in the optimized DWA algorithm, the robot normalizes the evaluation functions of various dimensions pre-set in the DWA algorithm, and then ratios the normalized results of the evaluation functions of the selected two dimensions. The method for obtaining the size relationship between the motion cost weighted parameters includes: the evaluation functions of various dimensions set by the robot are respectively an azimuth evaluation function, a distance evaluation function and a robot speed evaluation function; the motion cost weighted parameter applied by the robot to the azimuth evaluation function is the azimuth weight; the motion cost weighted parameter applied by the robot to the distance evaluation function is the distance weight; the motion cost weighted parameter applied by the robot to the robot speed evaluation function is the robot speed weight; wherein the azimuth evaluation function is used to evaluate the change in the angle difference between the end direction of the motion trajectory (the predicted trajectory) and the target point at the current set speed of the robot, specifically the change generated by the robot's linear velocity and angular velocity as independent variables, the target point can be the point closest to the robot on the edge line of the fitting figure used to mark the obstacle, and when the fitting figure is a circle, the azimuth evaluation function is used to describe the angle difference between the end direction of the motion trajectory (the predicted trajectory) and the tangent passing through the target point. The meaning of the distance evaluation function is the change in the distance between the point closest to the center of the fitting graph when the robot is at the end point of the predicted trajectory, specifically the change caused by the robot's linear velocity and angular velocity as independent variables, where the center of the fitting graph is used to represent the position information of the obstacle marked by the fitting graph, sampling points close to obstacles (nodes on the predicted trajectory) to ensure the robot's obstacle avoidance ability and reduce the probability of collision between the robot and obstacles. The robot speed evaluation function is used to describe the robot's current linear velocity change, specifically the change caused by the robot's linear velocity and angular velocity as independent variables, to promote the robot to quickly reach the target velocity.

[0049] For the evaluation function of each dimension, the normalization processing method includes: after the robot uses the sampling window to sample the vector velocity of the robot, based on the sampling data, a predicted trajectory is planned according to the kinematic model, which is equivalent to the sampled trajectory to be scored. Then, in the process of traversing each node of the predicted trajectory, the robot accumulates the function values ​​generated by the evaluation function at each node of the predicted trajectory, obtains the statistical sum value of the evaluation function, and obtains the maximum evaluation function value and the minimum evaluation function value generated by the evaluation function; then the ratio of the absolute value of the difference between the maximum evaluation function value and the minimum evaluation function value to the statistical sum value of the evaluation function is marked as the normalized result of the evaluation function. Within the error allowable range of the motion cost weighting parameter, the robot configures the product of the azimuth weight and the normalized result of the azimuth evaluation function to be equal to the product of the distance weight and the normalized result of the distance evaluation function; the robot also configures the product of the distance weight and the normalized result of the distance evaluation function to be equal to the product of the robot speed weight and the normalized result of the robot speed evaluation function. Preferably, among the azimuth weight, distance weight and robot speed weight, if one of the motion cost weighting parameters is selected as a preset constant, the other two are easy to obtain. For example, when the robot speed weight is set to a value of 1, the robot sets the ratio of the normalized result of the robot speed evaluation function to the normalized result of the azimuth evaluation function to be equal to the azimuth weight, and the robot sets the ratio of the normalized result of the robot speed evaluation function to the normalized result of the distance evaluation function to be equal to the distance weight.

[0050] Preferably, the error tolerance range of the motion cost weighting parameters is configured as follows: among the azimuth weight, the distance weight and the robot speed weight, the difference between any two motion cost weighting parameters does not exceed 10 to the negative sixth power.

[0051] Preferably, the normalized result of the azimuth evaluation function is used to convert the angle range output by the azimuth evaluation function into a reference angle range, the normalized result of the distance evaluation function is used to convert the distance between the end point of the predicted trajectory output by the distance evaluation function and the point on the edge line of the fitting figure into a reference distance range, and the normalized result of the robot speed evaluation function is used to convert the robot linear speed output by the robot speed evaluation function into a reference speed range, so as to adapt the fitting figure selected in step A to be converted into the corresponding cost map.

[0052] As an embodiment, in the optimized DWA algorithm, adjusting the size relationship between the motion cost weighting parameters according to the distance relationship between the robot and the obstacle marked by the currently selected fitting graphic includes:

[0053] When the robot detects that the center of the fitting figure screened in step A does not exist in the circular area with itself as the center and a radius of the preset safety distance, it is determined that the distance between all obstacles (including obstacles that have been detected and identified as state types and obstacles that are not within the maximum detection range) and the current position of the robot is greater than the preset safety distance, then the robot adjusts the azimuth weight and the distance weight to be greater than the robot speed weight, so that the robot accelerates to move toward the preset target point, but does not necessarily reach the preset target point, to form an optimal motion trajectory; the preset safety distance is preferably half of the robot's body radius or body width. The preset target point can be a point on the predicted trajectory planned according to the kinematic model of the DWA algorithm in the aforementioned embodiment, or the point closest to the robot on the edge line of the fitting figure used to mark the obstacle; in this embodiment, the smaller the robot speed weight is configured, the more it can weaken the impact of the robot's excessive linear speed, so that the robot can walk along the optimal motion trajectory at a higher speed, and the optimal motion trajectory is not only highly scored, but also can smoothly and effectively bypass obstacles.

[0054] When the robot detects that the minimum distance between the predicted trajectory and the center of the fitting graph selected in step A is less than the preset safety distance, it is determined that there is a path node on the predicted trajectory and a corresponding contour point on the obstacle marked by the fitting graph selected in step A, and the distance between the node is less than the preset safety distance, that is, the robot is relatively close to the obstacle when moving to the path node, and there is a risk of collision. The robot then adjusts both the distance weight and the robot speed weight to be greater than the azimuth angle weight, so that the robot can turn at a sufficiently large deflection angle when walking along the predicted trajectory to a path node to avoid colliding with the obstacle marked by the fitting graph selected in step A, thereby forming an optimal motion trajectory. In this embodiment, the smaller the azimuth angle weight is configured, the easier it is to weaken the influence of the deflection angle. The robot can deflect at a larger angle while walking along the optimal motion trajectory to safely pass near the obstacle.

[0055] When the robot is located at the previous path node of the path end point of the predicted trajectory, that is, when it is determined that the robot is located at a path node adjacent to the path end point planned by the DWA algorithm, the robot adjusts the azimuth angle weight to be greater than the distance weight and the robot speed weight at the same time, so that the robot starts to decelerate from the previous path node of the path end point of the predicted trajectory to the path end point, and does not rotate in place before moving to the path end point, so as to avoid the robot being trapped in the same path node and its surrounding area and unable to walk to the path end point. In this embodiment, the speed weight and the distance weight are set to be smaller, which weakens the influence of speed and distance, and can ensure that the distance between the body and the obstacle at the previous path node of the path end point is not too small. Therefore, the optimal motion trajectory obtained by the final score is not only a high score, but also can smoothly and effectively bypass obstacles.

[0056] It should be noted that, in this embodiment, the optimal motion trajectory is the planning path with the highest score among the weighted average results of the adjusted motion cost weighted parameters and the evaluation functions of the corresponding dimensions; specifically, for the three debugging methods of the size relationship between the aforementioned motion cost weighted parameters, the robot adds the product of the azimuth evaluation function and the adjusted azimuth weight, the product of the distance evaluation function and the adjusted distance weight, and the product of the robot speed evaluation function and the adjusted robot speed weight, and sets it as the scoring result corresponding to a predicted trajectory; wherein, each predicted trajectory corresponds to the evaluation function matching the above three dimensions, when the predicted trajectory changes or switches to a different predicted trajectory, the output results of the corresponding matching three dimensions of the evaluation function will also change, and the scoring result generated by the weighted average value thereof will also change. Provide effective protection for the collision-free motion of the robot.

[0057] The aforementioned embodiment calculates the weight coefficient in combination with the normalized result of the evaluation function, and adaptively adjusts the weight coefficient of the introduced evaluation function in the contact relationship between the robot and the obstacle, and the various positional relationships between the predicted trajectory (derived from the corresponding trajectory inferred by the kinematic model) and the obstacle (marked or circled by the fitting graph), thereby changing the score of the pre-planned trajectory and forming the optimal trajectory, so as to facilitate the subsequent selection of a planned path with a higher score that effectively avoids obstacles, and is not limited to a local path.

[0058] As an embodiment, after the robot fails to plan an obstacle avoidance path using the optimized DWA algorithm, when the robot does not touch the obstacle, if there is point cloud data reflected from the obstacle in the sampling window, that is, when the robot detects the obstacle, the robot uses the coordinates of the center of the fitting figure screened out in step A as the mean and the parameter positively correlated with the speed of the obstacle marked by the fitting figure as the standard deviation to establish a two-dimensional Gaussian distribution, so that when the fitting figure is a circle, a two-dimensional Gaussian distribution is constructed according to the coordinates, radius and movement speed of the center of the fitting figure, and then the edge line of the fitting figure is discretized into multiple points using the two-dimensional Gaussian distribution, and then these discrete points are respectively mapped to the grid map and marked as the grids occupied by the obstacle, wherein, in order to simplify the calculation complexity, the grid map The graph can be binarized so that the grid where the pixel value is greater than 128 in the grid map is marked as binary 0 to form a passable grid, and the grid where the pixel value is greater than 128 is marked as binary 1 to form an impassable grid, and a binary grid map composed of binary 0 and binary 1 is constructed and updated to the grid map; if necessary, the cost value of the grid in the grid neighborhood occupied by the obstacle (which can be represented by the size of the passable area or the length of the passable path) can be updated or increased according to the direction of the movement speed of the fitting graph, and the fitting graph is superimposed on the pre-constructed grid map to form a new cost map, and the movement behavior of the obstacle is predicted in advance to reduce the number of collisions between the robot and the dynamic obstacle. Then, a heuristic search algorithm (including A* algorithm) is used on the grid map to plan a path to bypass the grid occupied by the obstacle, and the obstacle avoidance path is determined to be planned using the heuristic search algorithm to achieve the effect of dynamic obstacle avoidance of the robot under the condition of global path planning.

[0059] Specifically, the method of using a heuristic search algorithm to plan a path that bypasses a grid occupied by an obstacle on the grid map includes: the robot first sets a path start point and a path end point on a global map coordinate system; then converts the coordinates of the current position and / or the coordinates of a preset target point to the global map coordinate system, the coordinates of the current position and / or the coordinates of the preset target point are located on the robot coordinate system, and the preset target point here may be located in the neighborhood of the robot's current position or at a position one body radius away from the robot's current position; the coordinate conversion method here, taking the robot's current position coordinates as an example, first converts the horizontal coordinate and vertical coordinate of the current position respectively according to the same preset scale factor. The robot coordinate system is converted, and then the converted horizontal and vertical coordinates are translated respectively, so that the origins of the robot coordinate system and the global map coordinate system coincide with each other, wherein the preset proportionality coefficient is determined by the ratio of the resolution of the robot coordinate system (how much physical distance is represented by one grid side length) to the resolution of the global map coordinate system (how much physical distance is represented by one grid side length). Preferably, one coordinate axis of the robot coordinate system is parallel to the same coordinate axis of the global map coordinate system, and then when the positive directions of one of the coordinate axes are in the same direction, the product of the offset of the origin of the two coordinate systems on the coordinate axis and the preset proportionality coefficient is used as the translation of the horizontal and vertical coordinates after the above conversion. Then, a heuristic search algorithm is used to plan a path that bypasses the grids occupied by obstacles, and multiple path nodes located in the global map coordinate system and their path extension direction information (including the deflection angle of the extension direction of each straight line segment or curve segment relative to the coordinate axis) are obtained to realize the use of the heuristic search algorithm to plan the obstacle avoidance path. In this embodiment, the path starting point and the path end point are the starting point and the end point of the obstacle avoidance path, respectively. The current position or preset target point of the robot is the search starting point specified by the heuristic search algorithm, so that the robot starts a neighborhood search from the search starting point, mainly recursively searching for grids not occupied by obstacles in the four neighborhoods or eight neighborhoods of the search starting point until the path end point is searched. Then the robot plans the obstacle avoidance path in advance.

[0060] As an embodiment, step A also includes: using the coordinate change of the center of the selected fitting graph to identify the state type of the obstacle marked by the fitting graph to distinguish between static obstacles and dynamic obstacles. Among them, the fitting graph is the overall information configured as an obstacle, and the coordinate change of the center of the fitting graph is the motion information configured as an obstacle; the coordinate change of the center of the selected fitting graph is expressed as the coordinate motion amount of the fitting graph used to mark the same obstacle (moving obstacle). In this embodiment, the point cloud data obtained by the robot is always reflected from the same obstacle, and the obstacle may be in a moving state. The coordinate of the center of the fitting graph used to mark the obstacle continues to change over a period of time, forming the movement speed of the obstacle, so that the obstacle avoidance path planned by the optimized DWA algorithm in the above embodiment is applicable to the local area, and the optimal motion trajectory is the local path planned in advance.

[0061] As an embodiment, the step A specifically includes: step A1, the robot segments the acquired point cloud data to obtain a plurality of point sets; wherein the point cloud data is specifically the position information of the obstacle point cloud, and when the point cloud data is represented by laser data (laser point cloud), each frame of laser data has a corresponding angle of a laser and the distance of the laser point, and is represented in the form of polar coordinates. After determining the robot's own posture, the laser data is transformed by trigonometric functions to obtain the coordinate position information of the obstacle. Generally, after the laser data of the current frame is acquired by the robot's laser radar, the laser data with a distance too far from the laser point is removed, and all laser points that meet the threshold range requirements are retained in the form of coordinates in the laser coordinate system to form a laser point cloud. Among them, the greater the distance of the laser point, the sparser the laser point cloud. Then, the robot groups the acquired point cloud data, and further segments the point cloud data of each group, and repeats this until each laser point is segmented into a corresponding group to obtain a plurality of point sets.

[0062] It should be noted that each point set can correspond to a fitted line segment, that is, a fitted straight line segment, and within the allowable error range, the two endpoints of the fitted straight line segment can be represented by the first point and the last point in the point set respectively. The first point and the last point in the point set are the point with the smallest index value and the point with the largest index value respectively, which are expressed as the two points with the farthest distance in the point set; therefore, the fitted straight line segment corresponding to each point set can be described together by the straight line equation corresponding to the straight line segment, the first point and the last point in the point set.

[0063] In step A2, the robot merges the point sets obtained in step A1 (since all laser points that meet the threshold range requirements obtained by the robot need to be grouped, the point sets obtained in step A1 can be understood as the final point sets obtained in step A1), that is, all the point sets repeatedly segmented in step A1 are merged to obtain a new point set, and fitted line segments are obtained, including fitted line segments corresponding to the merged point sets and fitted line segments corresponding to the point sets separated in step A1 that do not need to be merged; in order to avoid excessive segmentation of the point cloud data in step A1 resulting in too many fitted straight line segments, step A2 judges and merges the fitted straight line segments in turn, which is also manifested as the merging of corresponding point sets, and obtains a relatively small number of fitted line segments; when there are some fitted straight line segments that do not need to be merged, they are all used as the fitted line segments finally obtained in step A2 together with the fitted line segments merged in step A2, and each corresponds to a point set.

[0064] Step A3, the robot performs a circle fitting operation on the fitting line segment obtained in step A2, that is, performs a circle fitting operation on each point set after the merging process in step A2 and each point set not after the merging process in step A2, and obtains a preset circumscribed circle to fully mark and delimit the corresponding obstacles, including dynamic obstacles and static obstacles; but in the case where different preset circumscribed circles may exist to mark the same object, it is necessary to perform a circle merging process on the currently obtained preset circumscribed circle to replace the previously obtained preset circumscribed circle, and the fitting circle obtained by the circle merging process is used as the fitting circle obtained in step A3. Among them, each preset circumscribed circle obtained by the circle fitting operation needs to be processed by the circle merging process. The radius of dynamic obstacles in the actual environment will not be too large. In order to eliminate the influence of long corridors, walls, and obstacles with large volumes, this embodiment only considers performing a circle fitting operation on the fitting line segment to obtain a preset circumscribed circle, and then obtains a fitting circle through the circle merging process, which is equivalent to a circular obstacle, rather than simply using a fitting line segment to represent a dynamic obstacle. Generally, a line segment obstacle is used to represent a wall or a long corridor.

[0065] Step A4, construct a raster map using point cloud data. At this time, the point cloud data needs to be converted into map data first, and then the raster map is binarized according to accessibility so that each raster is represented by binary 0 or binary 1, and then the binarized raster map is eroded, and then the raster map after binarization and erosion is updated to a raster map, so as to represent the raster map in a more unified form that is convenient for image calculation operations.

[0066] It should be noted that the robot uses the Cartographer algorithm to build a grid map. The Cartographer algorithm is mainly responsible for processing data from the lidar, IMU, and odometer, and constructs a map based on these data. In the laser positioning and navigation scenario, the Cartographer framework is generally divided into front-end matching and back-end optimization. First, the front-end matching process is the process of creating a local map. By adding a series of point cloud filters, gyroscopes, and odometer data information for pose fusion, the fused pose is added to the local map. The matching strategy here is to match the current scanned laser frame image with the pre-scanned laser frame image for pose matching. However, when a large number of laser frame images are created, there will be error accumulation. At this time, the back-end loop detection is introduced for optimization, that is, the branch and bound algorithm. This algorithm divides the grid into several layers of depth (in layman's terms, it is divided into several layers of resolution), and matches the low resolution first and then the high resolution, which greatly shortens the loop detection time. Once the loop is established, optimization will be performed. The optimization method is to construct a pose graph. By forming respective constraints for gyroscope data, odometer data, laser data, and local map data, a polygonal loop is established, nodes on the loop are established, and optimization is performed through a sparse pose graph. The optimization method is to establish a nonlinear least squares equation for optimization, and then assign corresponding pixel values ​​to each grid of the map built by Cartographer to describe the actual environment distribution, including the distribution of obstacles, and finally obtain the original raster map to accept the aforementioned binarization and corrosion processing.

[0067] Step A5, the robot converts the center of the fitting circle obtained in step A3 (all preset circumscribed circles obtained by the robot in step A3 need to be merged, and the fitting circle finally obtained) to the map to compare with the coordinate index information of the grid map finally updated in step A4, and screens out the fitting circle whose center falls into the passable area of ​​the grid map to eliminate the fitting circle at the boundary of the grid map. In some embodiments, known obstacles can be eliminated and newly added obstacles can be marked with circles during the comparison with the grid map, and it is determined that the robot obtains the fitting graphic for marking obstacles, that is, obtains the position information of possible dynamic obstacles inside the grid map, wherein the fitting graphic is represented in the form of a fitting circle (circle).

[0068] Step A6: By obtaining the center of the fitting graph used to mark the obstacle at different times, the speed of the obstacle is calculated, and then the obstacle is identified as a static obstacle or a dynamic obstacle according to the speed of the obstacle. The fitting graph used to mark part or all of the same obstacle will change with the change of the corresponding fitting line segment, and the fitting line segment will also change with the change of the point set. The change here refers to the change in the position of the same obstacle reflected by the point cloud data to describe the motion state of the obstacle. The fitting graph used to mark part or all of the same obstacle in this embodiment can be expressed as a fitting circle used to encircle part or all of the contour of the same obstacle, and in most cases, it is a circle that encircles the entire contour of the obstacle; thus, the dynamic obstacle is judged by the real-time change of the center of the fitting circle selected in step A5.

[0069] In summary, due to the measurement accuracy of the laser, the obstacles are scattered into scattered points, but this embodiment integrates them into a fitting graph, and uses the fitting graph to represent the overall characteristics of the corresponding marked obstacles, predicting the movement behavior of the obstacles in advance, and avoiding the possibility of planning a path through the obstacles in extreme cases. Once the robot plans a path in the direction of motion of the dynamic obstacle, it is not easy for the robot to collide with the dynamic obstacle.

[0070] As an embodiment, in step A1, the point cloud data obtained 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 obstacle reflection position, and then two adjacent laser points are two adjacent laser points, one of which can be regarded as being located in the neighborhood of the other 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.

[0071] The robot segments the acquired point cloud data to obtain a plurality of point sets, including:

[0072] Step A11, in the process of the robot searching for laser points, the robot divides the laser point cloud into multiple laser point groups by using the change of the Euclidean distance between the two laser points; specifically, the method for the robot to divide the laser point cloud into multiple laser point groups by using the change of the Euclidean distance between the two laser points includes: Step A111, the robot calculates the Euclidean distance between the laser point currently searched and the laser point previously searched; if the Euclidean distance is less than a preset grouping distance threshold, the laser point currently searched and the laser point previously searched are grouped into the same laser point group; if the Euclidean distance is greater .... If the distance between the laser point currently searched and the laser point currently searched is greater than or equal to the preset grouping distance threshold, the laser point currently searched is classified into a new laser point group, and the laser point currently searched 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 a new laser point, and then updates the laser point currently searched to the laser point searched last time, and then executes step A111, until the robot calculates the Euclidean distance between each laser point and any other laser point and divides the laser point group according to the Euclidean distances between the corresponding two laser points, and then executes step A12.

[0073] As an embodiment, corresponding to step A11, Figure 2 As shown, the robot calculates the current laser point one by one With the previous laser point The Euclidean 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 The laser points in the vicinity of If the distance is less than the preset grouping distance threshold, the laser point and laser dot are grouped into the same group and marked as two adjacent laser points in the same laser point group; otherwise, Create a new laser point group S for the first point j , relative to the laser point The laser point group S j-1 The preset grouping distance threshold is used to represent the maximum distance that can be reached between two laser points in the same group of laser points that are pre-configured, or greater than the maximum distance. When the robot calculates the Euclidean distance between any two laser points, that is, calculates the Euclidean distance between each laser point and any other laser point and divides the laser point groups according to the Euclidean distances of the corresponding two laser points, the robot has traversed all the laser points and cannot continue to expand new laser points, and a number of grouped laser point groups S will be obtained. j (j=1,2,...,n,n≤m), j represents the number of laser point groups or the group sequence number; it should be noted that Figure 2In The laser points acquired by the robot or the laser points that meet the requirements among the acquired laser points are retained in the form of i It corresponds to the horizontal coordinate of the i-th laser point in the laser coordinate system, y i It corresponds to the ordinate of the i-th laser point in the laser coordinate system, and m is the total number of laser points, which is also equivalent to the number of laser points that the robot needs to search. Figure 2 Among the laser points in the j The deviation distance of the laser points in the y-axis direction relative to the origin (ox, oy) is smaller than that of the laser point group S j-1 The deviation distance of the laser point in the y-axis direction relative to the origin (ox, oy).

[0074] Step A12, the robot uses the least square method to fit a fitting line segment for the laser points in each laser point group. In this embodiment, each laser point group is fitted with a fitting line segment; in each laser point group, the robot marks the laser point farthest from the corresponding fitted line segment as the farthest point. When the robot calculates that the distance between the farthest point and the fitting line segment corresponding to the laser point group is greater than a 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 of the subsets, and determines that the laser point group meets the segmentation condition; wherein the preset segmentation distance threshold is used to indicate that The maximum distance from a laser point in the same group of laser points to its corresponding fitting straight line; within the laser point group, the robot classifies the laser points with smaller index values ​​than the farthest point into one subset, and classifies the laser points with larger index values ​​than the farthest point into another subset, and the farthest point can be classified into any one of the two subsets divided above, or simultaneously classified into each of the two subsets divided above; in some embodiments, the robot classifies the laser points with earlier search times than the farthest point into one subset, and classifies the laser points with later search times than the farthest point into another subset, and the robot searches / traverses the laser points in order of distance.

[0075] Step A13, the robot updates each subset to the laser point group described in step A12, and then executes step A12 until the distance between the farthest point corresponding to 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, that is, all the separated laser point groups or the subsets do not meet the segmentation condition; then the laser point groups with less than the preset number threshold are eliminated, and 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. The preset number threshold is preferably 3.

[0076] As an example, Figure 3 As shown, the robot first uses the least squares method to find the laser point group S to be segmented. j Fit a fitting line segment l within the laser point in m , that is, the laser point group S j Including laser points, the linear equation A is obtained by using the least squares method to fit the straight line m *x+B m *y+C m =0. Since the desired straight line is not unique, we only need to set C m It does not need to be equal to 0. The linear coefficient A is determined by the least squares method. m and B m , where the linear coefficient A m , Linear coefficient B m and the linear coefficient C m For the specific solution method, refer to the calculation formula associated with straight line fitting disclosed in step 5 of Chinese invention patent CN113253717A, which will not be repeated here. Then, find the distance straight line l in the laser point group m Farthest laser point If the laser point To the straight line segment l m The distance d k If the distance is greater than the preset split distance threshold, the laser point The laser point group is divided into two subsets S j , S j+1 , where the subset S j Existence Subset S j+1 Existence and are simultaneously classified into the subset S j and into the subset S j+1 ; On this basis, the aforementioned segmentation process is repeated for each subset in turn until all subsets do not meet the segmentation conditions; then the robot removes the laser point group with less than 3 laser points, and the corresponding fitted line segment is not called by the robot for subsequent circle fitting operations. On this basis, all point cloud data can be separated into several point sets according to the rules set in the aforementioned embodiment, and each point set corresponds to a fitted straight line. Then, in this embodiment, the first laser point and the last laser point in the laser point group and the corresponding fitted straight line equation are used to describe the corresponding fitted line segment. Within the allowable error range, the first laser point and the last laser point in the laser point group can be regarded as the two endpoints of the fitted line segment corresponding to the laser point group.

[0077] As an embodiment, in the step A2, the method for merging the point set includes: each time the robot searches for two fitting line segments from the fitting line segments finally obtained in step A13, detecting the attribute difference between the two fitting line segments, including the two endpoints of the fitting line segments, the intercept and the slope of the corresponding straight line equation; the two fitting line segments searched by the robot may be two adjacent fitting line segments, but not necessarily two parallel line segments; when the robot detects that the distance between the nearest endpoints of the two fitting line segments is less than a preset contour distance threshold, and the absolute value of the difference between the slopes of the two fitting line segments is less than a preset slope threshold, and the extension of the two fitting line segments When the distance between the intersection of the two 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 to determine that the two fitted line segments are respectively located on two separated obstacles; then, the robot merges the two laser point groups 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, and the new fitted line segment can represent an obstacle alone.

[0078] In order to avoid the problem of 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 point sets segmented in step A1 and their corresponding fitting line segments to obtain the final several fitting line segments; Figure 4 As shown, there are two adjacent straight line segments, and the corresponding straight line equations are expressed as: 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 straight line l m is the subset S j The fitting straight line obtained by fitting the laser points inside is the straight line l m+1 is the subset S j+1 First, determine whether the two straight line segments are close. This embodiment uses d 0 Indicates the shortest distance between the two straight line segments, and uses the preset contour distance threshold to indicate the distance threshold at which the two straight line segments are in the same obstacle. Figure 4 As shown, the straight line segment l m The last point on the line segment l m+1 The distance of the first point on the line is lm With straight line l m+1 The closest distance between 0 It means that when d 0 When the distance is less than the preset contour distance threshold, the straight line l is determined m With straight line l m+1 On the same straight line, the straight line segment l m The last point on the line segment l m+1 The first point on the j+1 The laser points in the j Among the laser points in the j , to achieve the subset S j The update of , also merges a new laser point group to avoid over-segmentation of the subset, and the straight line l can be fitted accordingly. m After all, the original straight line segment l m With straight line segment l m+1 are considered to be in the same straight line.

[0079] Preferably, for each fitting line segment, the two endpoints of the fitting line segment are respectively configured as the first laser point and the last laser point in the laser point group corresponding to the fitting line segment; each laser point group corresponds to a fitting line segment. In 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, so that the first laser point and the last laser point are representative in the laser point group where they are located, and have a sense of connectivity between two different laser point groups.

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

[0081] As an embodiment, in step A3, a circle fitting operation is performed on the fitting line segment, and a method for obtaining the fitting circle includes: the robot obtains a vector perpendicular to the fitting line segment and pointing to the origin of the laser coordinate system from each fitting line segment obtained in step A2, that is, the robot obtains a vector perpendicular to the fitting line segment and pointing to the origin of the laser coordinate system from each fitting line segment merged and processed in step A2 and each fitting line segment not merged and processed in step A2 (derived from the corresponding fitting line segment in each laser point group segmented in step A112), sets the angle between the vector and the horizontal axis as the horizontal deflection angle, and sets the vector The angle formed with the ordinate axis is set as the vertical deflection angle; then the robot sets the fitting line segment as a side of an equilateral triangle, which is generally a line connecting the first laser point and the last laser point of the laser point group corresponding to the fitting line segment; then the robot sets the circumcenter of the equilateral triangle (the center of the circumscribed circle of the equilateral triangle) to be separated from the origin of the laser coordinate system on both sides of the fitting line segment, or to be on the same side of the fitting line segment as the origin of the laser coordinate system; then, since an equilateral triangle is being processed, the product of the length of the fitting line segment and the tangent function value of 30 degrees is set as the circumscribed circle of the equilateral triangle. radius; then mark the product of half of the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the horizontal deflection angle as the preset horizontal axis offset coordinate, and determine that the preset horizontal axis offset coordinate is the coordinate offset of the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the horizontal axis; at the same time, the robot marks the product of half of the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the vertical deflection angle as the preset vertical axis offset coordinate, and determine that the preset vertical axis offset coordinate is the coordinate offset of the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the vertical axis; then use the preset vertical axis offset coordinate and The preset horizontal axis offset coordinate is used to calculate the coordinate position of the center point of the circumscribed circle of the equilateral triangle, that is, the robot uses the preset vertical axis offset coordinate to add the vertical coordinate of the midpoint of the fitting line segment to obtain the vertical coordinate of the center point of the circumscribed circle of the equilateral triangle. Similarly, the robot uses the preset horizontal axis offset coordinate to add the horizontal coordinate of the midpoint of the fitting line segment to obtain the horizontal coordinate of the center point of the circumscribed circle of the equilateral triangle. After determining the coordinate position of the center point of the circumscribed circle of the equilateral triangle, combined with the radius of the circumscribed circle of the equilateral triangle, the robot can determine the coverage area of ​​the circumscribed circle of the equilateral triangle in the laser coordinate system.Then, based on the coordinate position of the center of the circumscribed circle of the equilateral triangle, the robot expands the radius of the circumscribed circle of the equilateral triangle by a preset radius increment to obtain the preset circumscribed circle, and determines that the circle fitting operation is completed for the fitting line segment. Each fitting line segment obtained in step A2 corresponds to a preset circumscribed circle; wherein the set preset radius increment is equivalent to extending a circular boundary based on the circumscribed circle of the equilateral triangle to ensure that obstacles are included.

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

[0083] As an embodiment, in order to fully represent and wrap the obstacle, a circle fitting operation is performed on all the fitted line segments in step A2 (considered as the fitted line segments corresponding to the laser point group merged in step A2 and the fitted line segments corresponding to the laser point group that does not need to be merged (derived from the fitted line segments fitted in step A13)); see Figure 5 The method for performing a circle fitting operation on a laser point group or its corresponding fitting line segment includes: taking a straight line segment as an example, the first laser point in the laser point group corresponding to the straight line segment is The last laser point is can also be regarded as the two endpoints of the straight line segment; at the same time, the robot sets the normal vector of the straight line segment pointing to the origin of the laser coordinate system as like Figure 5 As shown, the robot uses the straight line segment as the base and designs an equilateral triangle whose circumcenter (center of the circle) is far away from the origin of the laser coordinate system (ox, oy), that is, the circumcenter of the equilateral triangle and the origin of the laser coordinate system are separated on both sides of the straight line segment. Then, the coverage area of ​​the circumscribed circle of the equilateral triangle is set farther, covering obstacles at positions farther than the center of the ranging sensor. After all, obstacles are not close to the robot. Specifically, in the circumscribed circle of the equilateral triangle, the radius of the circumscribed circle is The vector of the center of the circumcircle is expressed as The principle is that in the aforementioned embodiment, the product of half the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the horizontal deflection angle is marked as the preset horizontal axis offset coordinate, and the preset horizontal axis offset coordinate is determined to be the coordinate offset between the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the horizontal axis; at the same time, the robot marks the product of half the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the vertical deflection angle as the preset vertical axis offset coordinate, and determines that the preset vertical axis offset coordinate is the coordinate offset between the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the vertical axis. Furthermore, in order to ensure that the circle made can fully represent the obstacle or even encircle the obstacle, it is necessary to expand the radius of the circumscribed circle of the equilateral triangle by a preset radius increment r. enlarge , with r+r enlarge Indicates the radius of the final preset circumscribed circle. Indicates the coordinates of the center point of the preset circumscribed circle.

[0084] On the basis of the above-mentioned embodiment, in step A3, the method for merging the preset circumscribed circles includes: when the robot detects that there are two preset circumscribed circles in an inclusion relationship, retaining the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles, and eliminating the preset circumscribed circle with a relatively small radius among the two preset circumscribed circles, and setting the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles as a fitting circle, so that the fitting circle marks or encircles an obstacle, and then it can be determined which obstacle the fitting circle is used to mark, which is convenient for subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle, and the corresponding identification information is marked in the robot's memory to determine which obstacle the fitting circle is used to mark, which is convenient for subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle. When the robot detects that two preset circumscribed circles intersect, the line connecting the centers of the two preset circumscribed circles is configured as a fitting line segment, and then the fitting line segment is subjected to a circle fitting operation to obtain a reference circumscribed circle; then the radius of the reference circumscribed circle is added to the relatively larger radius of the two preset circumscribed circles before the aforementioned circle fitting operation to obtain a reference radius; when it is detected that the reference radius is smaller than the preset allowable radius, the center of the reference circumscribed circle is retained, specifically, the coordinates of the center of the circle are recorded, and the two preset circumscribed circles and the reference circumscribed circle before the circle fitting operation are eliminated, and then the center of the reference circumscribed circle is taken as the center of the circle and the reference radius is taken as the center of the circle. A circle is set with the diameter as the radius and the circle is marked as the fitting circle, so that the two preset circumscribed circles before the circle fitting operation are merged into the fitting circle, so that a fitting circle is used to mark or circle a corresponding obstacle, and the identification information is marked in the memory of the robot to determine which obstacle the fitting circle is used to mark, so as to facilitate the subsequent calculation of the movement speed of the fitting circle used to mark the same obstacle; in summary, the different fitting circles obtained in step A3 mark or circle different obstacles respectively, so as to avoid different circles (such as two preset circumscribed circles) representing the same obstacle.

[0085] Corresponding to Figure 6 In the process of merging all preset circumscribed circles, the two adjacent preset circumscribed circles ( Figure 6 The relationship between the center and radius of the circle in the figure is processed accordingly: when two preset circumscribed circles are intersecting, the center c of the two preset circumscribed circles is connected. 1 、c 2 , for c 1 and c 2 The above circle fitting operation is performed on the line connecting the two to obtain a circle with the center c 3 The new preset circumscribed circle is marked as the reference circumscribed circle, and the radius r of the reference circumscribed circle is 3 With the center of the circle as c 2 The radius r of the preset circumscribed circle 2If the added radius r is less than the preset allowable radius, the coordinate c of the center of the reference circumscribed circle is retained. 3 And radius r are used as components of the fitting circle to cover the laser coordinate system.

[0086] As an embodiment, the step A4 specifically includes: using the position information occupied by the obstacle represented by the point cloud data to assign corresponding pixel values ​​to each grid of the grid map, then marking the grid with a pixel value greater than a preset pixel threshold as a passable grid, and marking the grid with a pixel value less than or equal to the preset pixel threshold as an inaccessible grid, and then obtaining a binary grid map, and determining that the original grid map has been binarized; specifically, after the robot uses the Cartographer algorithm pre-stored in the body to build the grid map, it determines the position information occupied by the obstacle represented by the 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, otherwise marking the grid where the pixel point is located as binary 1 to form an inaccessible grid, constructing a binary grid map consisting of binary 0 and binary 1, and updating it to the grid map to simplify the representation form of the map and the filled internal environment information. Then, the robot marks the passable grids in the associated neighborhood of each impassable grid in the binarized grid map as impassable grids according to the preset point cloud conversion error, and obtains the grid map after corrosion processing; wherein the grid area of ​​the associated neighborhood of each impassable grid is positively correlated with the grid area involved in the preset point cloud conversion error. Preferably, during the corrosion processing, the robot updates all binary 0s in the twenty-four neighborhoods centered on each grid assigned with binary 1 to binary 1 in the grid map, that is, in the two adjacent circles of grids outside each grid assigned with binary 1, all binary 0s are updated to binary 1 to complete the corrosion processing of the grid map, wherein the area covered by the twenty-four neighborhoods is a grid area with 5 rows and 5 columns except the center, which can be recorded as the effective corrosion area of ​​this corrosion processing, or understood as a corrosion radius equal to two grids, and the specific corrosion radius can be adjusted according to the environment in which the robot is located so as to identify dynamic obstacles in a larger effective area. Then, the grid map that has undergone the aforementioned binarization processing and the aforementioned corrosion processing is updated to the grid map, and the area composed of the passable grids is marked as the passable area in the grid map. At the same time, the coordinate index values ​​of the passable grids in the grid map are recorded to form a set of passable points. In fact, the coordinate information of each grid in the global map coordinate system is saved in the form of an index value; then step A5 is executed.

[0087] As an embodiment, in step A5, the robot projects the center of the fitting circle obtained in step A3 from the laser coordinate system to the grid map updated at last in step A4, so as to convert the centers of all fitting circles from the form of laser points to the form of map index values, and obtain the coordinate index value of the center of the corresponding fitting circle in the global map coordinate system of the grid map, which actually saves the coordinate information of the center of the fitting circle in the global map coordinate system in the form of index values; preferably, the X-axis of the laser coordinate system is perpendicular or parallel to the X-axis of the global map coordinate system. 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 fitting circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitting 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 fitting circle is a fitting figure whose center falls into the passable area of ​​the grid map, then the robot can screen out the fitting circle whose center falls into the passable area of ​​the grid map and mark the fitting circle as the fitting figure for marking obstacles; when step A4 is When the coordinate index value of the center of the fitted circle obtained in A3 in the global map coordinate system of the grid map is not equal to any coordinate index value recorded in step A4, the center of the fitted circle is removed from the grid map, and the position points that are not within the boundary frame range of the grid map are removed, specifically, the center of the circle at the boundary position between the obstacle occupied area and the passable area is removed, and the remaining is the position information of the dynamic obstacles in the passable area of ​​the map, because the dynamic window can be called to detect the obstacles in motion and the point cloud data reflected back in the passable area.

[0088] As an embodiment, in step A6, the method of obtaining the center of the fitting figure for marking 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 according to the speed of the obstacle includes: the robot marks the coordinates of the center of the fitting figure for marking the obstacle screened out at the first moment in step A5 (the center of the fitting circle generated at the first moment under the premise of marking the same obstacle) as the first center coordinates; then, the robot marks the coordinates of the center of the fitting figure for marking the same obstacle screened out at the second moment in step A5 (the center of the fitting circle generated at the second moment under the premise of marking the same obstacle) as the second center coordinates; after the robot executes step A5, the basis for extracting the fitting circle for marking the same obstacle from the coordinate index information of the grid map last updated in step A4 is the correspondence between the fitting circle and the marked obstacle, and the problem of different fitting circles marking the same obstacle can be overcome by executing the circle merging process of step A3, and the corresponding relationship 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., the circle merging process).

[0089] The robot marks the time difference between the first moment and the second moment as the obstacle movement time difference; the obstacle movement time difference can be regarded as recording the adjacent time intervals passed by the characteristic points of the same obstacle (corresponding to the center of the fitting circle in the above embodiment), which can be specifically set according to the size, weight, friction force and other physical factors of the actual detected obstacle. Therefore, the robot marks the distance between the first center coordinate and the second center coordinate as the movement distance of the obstacle.

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

[0091] 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 in the fitting circle used to mark the static obstacle and its corresponding laser point group.

[0092] In summary, by screening out suitable fitting circles to calculate the overall characteristics of obstacles of different states and their motion information, it is convenient to predict the motion behavior of obstacles in advance and reduce the collision between the robot and dynamic obstacles.

[0093] The protection scope of the present invention is not limited to the above-mentioned embodiments. Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the scope and spirit of the present invention. If these changes and modifications fall within the scope of the claims of the present invention and their equivalents, the present invention is intended to include these changes and modifications.

Claims

1. A robot obstacle avoidance path planning method, wherein the robot is equipped with a ranging sensor for acquiring point cloud data; characterized in that: The robot obstacle avoidance path planning method includes the following steps: Step A: The robot extracts a fitting graphic for marking an obstacle from the point cloud data to surround the obstacle; at the same time, the robot constructs a grid map; then the robot compares the center of the fitting graphic with the grid map, and selects a fitting graphic whose center falls within the passable area of ​​the grid map; Step B: Based on the mapping of the currently selected fitting graph, the optimized DWA algorithm is called to plan the obstacle avoidance path; Or when the optimized DWA algorithm fails to plan the obstacle avoidance path, the heuristic search algorithm is called to plan the obstacle avoidance path; Step A also includes: using the coordinate change of the center of the selected fitting graph to identify the state type of the obstacle marked by the fitting graph to distinguish between a static obstacle and a dynamic obstacle; The step A specifically comprises: Step A1: The robot segments the acquired point cloud data to obtain a plurality of point sets; Step A2, merging the point set obtained in step A1 to obtain a fitting line segment; Step A3, performing a circle fitting operation on the fitting line segment obtained in step A2 to obtain a preset circumscribed circle; and then performing a circle merging operation on the preset circumscribed circle to obtain a fitting circle; Step A4: construct a raster map using the point cloud data, then binarize the raster map according to the accessibility, then erode the binarized raster map, and then update the binarized and eroded raster map to the raster map; Step A5, converting the center of the fitting circle obtained in step A3 to the map to compare with the coordinate index information of the grid map finally updated in step A4, screening out the fitting circle whose center falls into the passable area of ​​the grid map, and determining that the robot obtains the fitting graphic for marking the obstacle; Step A6: By obtaining the center of the fitting graph used to mark the obstacle at different times, the speed of the obstacle is calculated, and then the obstacle is identified as a static obstacle or a dynamic obstacle according to the speed of the obstacle.

2. The robot obstacle avoidance path planning method according to claim 1, characterized in that: The step B specifically comprises the following steps: Step B1: After the robot selects the corresponding fitting graph by executing step A, the evaluation function of the corresponding dimension in the DWA algorithm is introduced and the corresponding weighting parameter is adjusted. Then, the obstacle avoidance path is planned based on the mapping of the currently selected fitting graph within the sampling window of the DWA algorithm, so as to realize the planning of the obstacle avoidance path by calling the optimized DWA algorithm. Step B2: When the robot fails to plan an obstacle avoidance path using the optimized DWA algorithm, when the robot detects an obstacle, the robot plans an obstacle avoidance path using a heuristic search algorithm based on the mapping of the fitting graph selected in step A within the grid map.

3. The robot obstacle avoidance path planning method according to claim 2, characterized in that: In step B, the robot sets a sampling window, the size of which is set to limit the vector speed of the robot, and the sampling window is set to cover the entire outline of the obstacle when the robot detects the obstacle; The method for the robot to call the optimized DWA algorithm includes: the robot uses a sampling window to sample the vector velocity of the robot; then, the robot normalizes the evaluation functions of various dimensions pre-set in the DWA algorithm, and then compares the normalized processing results of the evaluation functions of the two selected dimensions to obtain the size relationship between the motion cost weighted parameters; then, the size relationship between the motion cost weighted parameters is adjusted according to the distance relationship between the robot and the obstacle marked by the currently screened fitting graphic, and then the sampling data of the sampling window is scored based on the size relationship between the adjusted motion cost weighted parameters to obtain the optimal motion trajectory, which corresponds to the obstacle avoidance path, so that the robot does not collide with obstacles when walking along the optimal motion trajectory.

4. The robot obstacle avoidance path planning method according to claim 3, characterized in that: In the step B, it also includes: the robot controls the fitting graph screened out in step A to expand and discretize into multiple points, obtains mapping information of the obstacle marked by the fitting graph within the sampling window, so as to form a cost map of the obstacle in its speed direction; and then extracts the distance between the robot and the obstacle marked by the fitting graph screened out in step A in real time from the sampling information, so that the path planned by the DWA algorithm on the cost map avoids the obstacle in motion; The greater the speed of the obstacle marked by the fitting graph, the larger the coverage area formed by the fitting graph after the expansion process; When the vector velocity of the robot includes the linear velocity and angular velocity of the robot, after scoring the sampling data of the sampling window based on the size relationship between the adjusted motion cost weighted parameters, the robot selects the speed combination with the highest score to plan a motion trajectory suitable for the robot to move according to the speed combination.

5. The robot obstacle avoidance path planning method according to claim 4, characterized in that: In step B1, it also includes: within the preset sampling time, when the robot moves at the currently set speed, if the distance between the robot and the center of the fitting figure screened out in step A is a preset safety distance, or there is a fitting figure screened out in step A on one side of the robot but the robot does not collide with an obstacle marked by a corresponding mark of the fitting figure on the side, then stop calling the optimized DWA algorithm; When the distance between the robot's current position and the center of the fitting figure used to mark the obstacle is less than or equal to the preset safety distance, if the robot does not change its current forward direction or keeps walking in the current forward direction until it collides with the obstacle, it is determined that the robot has failed to plan the obstacle avoidance path using the optimized DWA algorithm, and then stops calling the optimized DWA algorithm.

6. The robot obstacle avoidance path planning method according to claim 4, characterized in that: In the optimized DWA algorithm, the robot normalizes the evaluation functions of each dimension preset in the DWA algorithm, and then compares the normalized results of the evaluation functions of the two selected dimensions. The method for obtaining the size relationship between the motion cost weighted parameters includes: The evaluation functions of various dimensions of the robot setting are azimuth evaluation function, distance evaluation function and robot speed evaluation function; the motion cost weighting parameter applied by the robot to the azimuth evaluation function is the azimuth weight; the motion cost weighting parameter applied by the robot to the distance evaluation function is the distance weight; the motion cost weighting parameter applied by the robot to the robot speed evaluation function is the robot speed weight; For the evaluation function of each dimension, the normalization processing method includes: after the robot uses the sampling window to sample the vector velocity of the robot, based on the sampling data, the predicted trajectory is planned according to the kinematic model; in the process of traversing each node of the predicted trajectory, the robot accumulates the function value generated by the evaluation function at each node of the predicted trajectory, obtains the statistical sum value of the evaluation function, and obtains the maximum evaluation function value and the minimum evaluation function value generated by the evaluation function; then the absolute value of the difference between the maximum evaluation function value and the minimum evaluation function value and the ratio of the statistical sum value of the evaluation function are marked as the normalized result of the evaluation function; Within the allowable error range of the motion cost weighting parameter, the robot configures the product of the azimuth weight and the normalized result of the azimuth evaluation function to be equal to the product of the distance weight and the normalized result of the distance evaluation function; the robot also configures the product of the distance weight and the normalized result of the distance evaluation function to be equal to the product of the robot speed weight and the normalized result of the robot speed evaluation function.

7. The robot obstacle avoidance path planning method according to claim 6, characterized in that: In the optimized DWA algorithm, the relationship between the motion cost weighting parameters is adjusted according to the distance relationship between the robot and the detected obstacle, including: When the robot detects that the center of the fitting figure selected in step A does not exist in the circular area with itself as the center and a radius of the preset safety distance, the robot adjusts both the azimuth angle weight and the distance weight to be greater than the robot speed weight, so that the robot accelerates to move toward the preset target point; When the robot detects that the minimum distance between the predicted trajectory and the center of the fitting graph selected in step A is less than the preset safety distance, the robot adjusts both the distance weight and the robot speed weight to be greater than the azimuth angle weight, so that the robot does not collide with the obstacle marked by the fitting graph selected in step A during the turning process; When the robot is located at a path node before the end point of the path of the predicted trajectory, the robot adjusts the orientation angle weight to be greater than the distance weight and the robot speed weight at the same time, so that the robot decelerates and moves to the end point of the path; The score of each predicted trajectory is configured to be equal to the weighted average of the evaluation functions of the corresponding dimension.

8. The robot obstacle avoidance path planning method according to claim 2 or 7, characterized in that: After the robot fails to plan an obstacle avoidance path using the optimized DWA algorithm, when the robot does not touch the obstacle, if there is point cloud data reflected from the obstacle in the sampling window, the robot uses the coordinates of the center of the fitting figure screened out in step A as the mean and the parameter positively correlated with the speed of the obstacle marked by the fitting figure as the standard deviation to establish a two-dimensional Gaussian distribution, and then uses the two-dimensional Gaussian distribution to discretize the edge line of the fitting figure into multiple points, and then maps these discrete points to the grid map and marks them as grids occupied by obstacles, and then uses the heuristic search algorithm to plan a path on the grid map to bypass the grids occupied by obstacles, and determines to use the heuristic search algorithm to plan the obstacle avoidance path.

9. The robot obstacle avoidance path planning method according to claim 8, characterized in that: The method of planning a path to bypass the grids occupied by obstacles on the grid map using a heuristic search algorithm includes: The robot first sets the path starting point and path end point in the global map coordinate system; then converts the coordinates of the current position and / or the coordinates of the preset target point to the global map coordinate system, and then uses the heuristic search algorithm to plan a path that bypasses the grid occupied by the obstacle, and obtains multiple path nodes and their path extension direction information located in the global map coordinate system, so as to realize the use of the heuristic search algorithm to plan an obstacle avoidance path, wherein the path starting point and path end point are the starting point and end point of the obstacle avoidance path respectively, and the robot's current position or preset target point is the search starting point specified by the heuristic search algorithm.

10. The robot obstacle avoidance path planning method according to claim 1, characterized in that: 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 robot segments the acquired point cloud data to obtain a plurality of point sets, including: Step A11: When the robot searches for laser points, the robot divides the laser point cloud into multiple laser point groups by using the change of the Euclidean distance between two laser points. Step A12, the robot uses the least square method to fit a fitting line segment for the laser points in each laser point group; in each laser point group, the robot marks the laser point farthest from the corresponding fitted line segment as the farthest point, and when the robot calculates that the distance between the farthest point and the fitting line segment corresponding to 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, wherein, in the laser point group, the robot classifies the laser points with smaller index values ​​than the farthest point into one subset, and classifies the laser points with larger index values ​​than the farthest point into another subset; Step A13, the robot updates each subset to the laser point group described in step A12, and then executes step A12 until the distance between the farthest point corresponding to 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, and then the laser point groups with the number of laser points less than the preset number threshold are eliminated, and 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.

11. The robot obstacle avoidance path planning method according to claim 10, characterized in that: In the step A11, the method in which the robot divides the laser point cloud into a plurality of laser point groups by using the change of the Euclidean distance between two laser points includes: Step A111, the robot calculates the Euclidean distance between the laser point currently searched and the laser point previously searched; if the Euclidean distance is less than the preset grouping distance threshold, the laser point currently searched and the laser point previously searched are grouped into the same laser point group; if the Euclidean distance is greater than or equal to the preset grouping distance threshold, the laser point currently searched is grouped into a new laser point group, and the laser point currently searched is marked as the first laser point in the new laser point group; then step A112 is executed; Step A112, the robot searches for a new laser point, then updates the laser point currently searched as the laser point last searched, and then executes step A111, until the robot calculates the Euclidean distance between each laser point and any other laser point and divides the laser point group according to the Euclidean distances between the corresponding two laser points, and then executes step A12.

12. The robot obstacle avoidance path planning method according to claim 10, characterized in that: In step A2, the method for merging the point set includes: Whenever the robot searches for two fitting line segments from all the fitting line segments obtained in step A13, if the robot detects that the distance between the nearest endpoints of the two fitting line segments is less than the preset contour distance threshold, and the absolute value of the difference between the slopes of the two fitting line segments is less than the preset slope threshold, and the distance between the intersection of the extension lines of the two fitting line segments and the same coordinate axis is less than the preset intercept threshold, it is determined that the two fitting line segments are in a straight line; then, the two laser point groups corresponding to the two fitting line segments are merged into a new laser point group, and the laser points in the merged laser point group are fit into a new fitting line segment using the least squares method.

13. The robot obstacle avoidance path planning method according to claim 12, characterized in that: For each fitting line segment, the two endpoints of the fitting line segment are respectively configured as the first laser point and the last laser point in the laser point group corresponding to the fitting line segment; each laser point group corresponds to a fitting line segment; In the same laser point group, the distance between the first laser point and the last laser point is the maximum value of the distances between any two laser points.

14. The robot obstacle avoidance path planning method according to claim 13, characterized in that: In step A3, the method of performing a circle fitting operation on the fitting line segment includes: The robot obtains a vector perpendicular to each fitting line segment obtained in step A2 and pointing to the origin of the laser coordinate system, sets the angle between the vector and the abscissa axis as the horizontal deflection angle, and sets the angle between the vector and the ordinate axis as the vertical deflection angle; Then, the fitting line segment is set as a side of an equilateral triangle, and then the circumcenter of the equilateral triangle is set to be separated from the origin of the laser coordinate system on both sides of the fitting line segment, or is set to be located on the same side of the fitting line segment as the origin of the laser coordinate system; Then, the product of the length of the fitting line segment and the tangent function value of 30 degrees is set as the radius of the circumscribed circle of the equilateral triangle; then, the product of half of the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the horizontal deflection angle is marked as a preset horizontal axis offset coordinate, and the preset horizontal axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the horizontal axis; at the same time, the product of half of the radius of the circumscribed circle of the equilateral triangle and the cosine function value of the vertical deflection angle is marked as a preset vertical axis offset coordinate, and the preset vertical axis offset coordinate is determined to be the coordinate offset of the midpoint of the fitting line segment and the center of the circumscribed circle of the equilateral triangle on the vertical axis; then, the preset vertical axis offset coordinate and the preset horizontal axis offset coordinate are used to calculate the coordinate position of the center of the circumscribed circle of the equilateral triangle; Based on the coordinate position of the center of the circumscribed circle of the equilateral triangle, the radius of the circumscribed circle of the equilateral triangle is enlarged by a preset radius increment to obtain the preset circumscribed circle, and it is determined that the circle fitting operation is completed for the fitting line segment.

15. The robot obstacle avoidance path planning method according to claim 14, characterized in that: In step A3, the method of performing circle merging processing on the preset circumscribed circle includes: When the robot detects that there are two preset circumscribed circles in an inclusion relationship, the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles is retained, and the preset circumscribed circle with a relatively small radius among the two preset circumscribed circles is eliminated, and the preset circumscribed circle with a relatively large radius among the two preset circumscribed circles is set as a fitting circle, so that the fitting circle marks or encircles an obstacle; When the robot detects that there are two preset circumscribed circles intersecting, the line connecting the centers of the two preset circumscribed circles is configured as a fitting line segment, and then the fitting line segment is subjected to the aforementioned circle fitting operation to obtain a reference circumscribed circle; then the radius of the reference circumscribed circle is added to the relatively larger radius of the two preset circumscribed circles before the aforementioned circle fitting operation to obtain a reference radius; when it is detected that the reference radius is smaller than the preset allowable radius, the center of the reference circumscribed circle is retained, and then a circle is set with the center of the reference circumscribed circle as the center and the reference radius as the radius, and the circle is marked as the fitting circle, so that the fitting circle marks or encircles an obstacle.

16. The robot obstacle avoidance path planning method according to claim 1, characterized in that: The step A4 specifically includes: using the position information of the obstacle represented by the point cloud data to assign corresponding pixel values ​​to each grid of the grid map, and then marking the grid with a pixel value greater than a preset pixel threshold as a passable grid, and marking the grid with a pixel value less than or equal to the preset pixel threshold as an impassable grid, and then obtaining a binarized grid map, and determining that the original grid map has been binarized; Then, according to the preset point cloud conversion error, the passable grids in the associated neighborhood of each impassable grid in the binary grid map are marked as impassable grids, and the grid map after corrosion processing is obtained; wherein the grid area of ​​the associated neighborhood of each impassable grid is positively correlated with the grid area involved in the preset point cloud conversion error; Then, the grid map that has been binarized and eroded is updated to the grid map, and the area composed of passable grids is marked as the passable area in the grid map, and the coordinate index value of the passable grid in the grid map is recorded; then step A5 is executed.

17. The robot obstacle avoidance path planning method according to claim 16, characterized in that: In step A5, the robot projects the center of the fitting circle obtained in step A3 from the laser coordinate system to the grid map finally updated in step A4, and obtains the coordinate index value of the center of the corresponding fitting 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 fitting circle in the global map coordinate system of the grid map; when the coordinate index value of the center of the fitting 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 fitting circle is the fitting figure whose center falls into the passable area of ​​the grid map screened out in step A, and determine to screen out the fitting circle whose center falls into the passable area of ​​the grid map and mark the fitting circle as the fitting figure for marking the obstacle; when the coordinate index value of the center of the fitting circle obtained in step A3 in the global map coordinate system of the grid map is not equal to any one of the coordinate index values ​​recorded in step A4, remove the center of the fitting circle from the grid map.

18. The robot obstacle avoidance path planning method according to claim 17, characterized in that: In step A6, the method of obtaining the center of the fitting 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 according to the speed of the obstacle includes: The robot marks the coordinates of the center of the fitting figure screened out in step A5 at the first moment as the first center coordinates; then, the robot marks the coordinates of the center of the same fitting figure screened out in step A5 at the second moment as the 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 the ratio of the obstacle's moving distance to the obstacle's moving time difference is calculated to obtain the obstacle's speed; When the robot determines that the speed of the obstacle is greater than the preset speed threshold, it determines that the obstacle is a dynamic obstacle, and then repeats steps A1 to A6 to track the motion 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, it determines that the obstacle is a static obstacle.

Citation Information

Patent Citations

  • Indoor mobile robot local path planning method based on dynamic obstacle motion information

    CN113253717A

  • Three-dimensional spatial path planning method based on dynamic spherical window

    CN108958282A

  • Mobile robot path planning method in dynamic environment

    CN109782763A

Cited By

  • Building construction robot walking system based on intelligent obstacle avoidance and path planning algorithm

    CN120891826A

  • Construction robot walking system based on intelligent obstacle avoidance and path planning algorithm

    CN120891826B