Robot dynamic obstacle avoidance method and system

Abstract

The robot dynamic obstacle avoidance method and system of the present invention firstly causes the robot to walk through a plurality of key points selectable by the user and obtains a global path according to each key point, then obtains a local map of each period based on a two-dimensional map mapped with the global path and environmental information to obtain a local path of the robot corresponding to each period, and obtains a travel route and a travel speed in each period based on the local path and the speed state prediction of the robot to perform obstacle avoidance navigation. The present invention can quickly control the robot to avoid obstacles, improve the accuracy of obstacle avoidance, and realize the safe and efficient movement of controlling the robot to avoid obstacles.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence, and in particular to a robot dynamic obstacle avoidance method and system. Background Art

[0002] As robot technology matures, robots are widely used in production lines, industrial services, indoor mobile customer services, and dangerous operations. The key requirement for mobile robots to achieve autonomous navigation is obstacle avoidance. Obstacle avoidance means that when a mobile robot senses static and dynamic objects that hinder its passage through sensors based on the state information of the obstacles collected during walking, it avoids obstacles in a certain way and finally reaches the target point. The existing robot obstacle avoidance method cannot reasonably plan the obstacle avoidance path, which seriously affects the safe and efficient movement of the robot to avoid obstacles. Summary of the invention

[0003] In view of the above-mentioned shortcomings of the prior art, the purpose of the present invention is to provide a robot dynamic obstacle avoidance method and system, which is used to solve the problem that the robot obstacle avoidance method in the prior art cannot reasonably plan the obstacle avoidance path, which seriously affects the robot's safe and efficient movement to avoid obstacles.

[0004] To achieve the above-mentioned purpose and other related purposes, the present invention provides a robot dynamic obstacle avoidance method, the method comprising: obtaining a global path according to the position information of multiple key points walked by the robot; the global path comprises: a starting point and a target end point; discretizing the global path, and mapping the global path point sequence obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, selecting the local planning end point in each cycle from the global path point in the local map corresponding to each cycle, and obtaining the local path of the robot corresponding to each cycle based on the local path algorithm; enumerating one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and screening the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the travel route and travel speed of the next cycle, so as to navigate the robot to the end point.

[0005] In one embodiment of the present invention, the global path is discretized, and the sequence of global path points obtained by discretization is mapped to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map, including: discretizing the global path into a plurality of global path points whose distances to adjacent points are preset distance parameters, and obtaining a sequence of global path points; mapping the sequence of global path points to the two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; wherein the two-dimensional mapping map includes: the location information of each obstacle and each global path point in the map coordinates.

[0006] In one embodiment of the present invention, based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, the local planning end point in each cycle is selected from the global path points in the local map corresponding to each cycle, and based on the local path algorithm, the local path of the robot corresponding to each cycle is obtained. The method includes: based on the two-dimensional mapping map, the current position of the robot is mapped to obtain a local map describing the surrounding environment in the current cycle; the local map includes: one or more global path points and the location information of obstacles; based on the current position and current speed state of the robot, the local planning end point of the current cycle is selected from the global path points in the local map corresponding to the current cycle; based on the local path algorithm, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is solved.

[0007] In one embodiment of the present invention, the method of selecting the local planning end point of the current cycle from the global path points in the local map corresponding to the current cycle based on the current position and current speed state of the robot includes: selecting a global path point in the corresponding cycle as the initial local planning end point based on the current position and current speed state of the robot; judging whether the initial local planning end point is occupied by an obstacle; if so, performing a target end point judgment process to select a new global path point as the local planning end point; if not, taking the initial local planning end point as the local planning end point; wherein the target end point judgment process includes: judging whether the initial local planning end point is the target end point; if so, selecting a global path point between the current position of the robot and the initial local planning end point and not occupied by an obstacle as the local planning end point based on a target end point selection rule; if not, selecting a global path point between the initial local planning end point and the global path point in the local map that is farthest from the current position of the robot and includes the global path point that is farthest from the current position as the local planning end point based on a non-target end point selection rule.

[0008] In one embodiment of the present invention, the target endpoint selection rule includes: selecting the local planning endpoint from the global path points that are between the current position of the robot and the initial local planning endpoint and are not occupied by obstacles in order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint; and / or, the non-target endpoint selection rule includes: selecting the global path points between the initial local planning endpoint and the global path points in the local map that are farthest from the current position of the robot and including the global path points at the farthest distance from the current position as the local planning endpoint in order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint.

[0009] In one embodiment of the present invention, the local path algorithm based on the current position of the robot and the local planning end point of the current cycle is used to solve the local path of the robot corresponding to the current cycle, including: based on the improved Astar core cost function, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is solved; wherein the improved Astar core cost function includes: the actual distance weight from the starting point of the current cycle to the current position, the distance weight from the current position to the nearest obstacle, the nearest distance weight from the current position to the last solved local path, and the estimated value weight from the current position to the local planning end point.

[0010] In one embodiment of the present invention, the enumeration of one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and screening the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the route and travel speed of the next cycle, so as to navigate the robot to the end point, includes: based on the collected current speed state of the robot, enumerating one or more predicted speed states corresponding to the current cycle; based on the set simulation time, simulating each predicted speed state to obtain one or more initial travel routes; based on the initial screening condition, screening one or more initial screening travel routes from the initial travel route; if the initial screening condition is If the number of the first-screened routes is at least two, then based on the scoring rules related to the local path, each of the first-screened routes is scored, and the highest-scoring screened route is set as the route for the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the route and the travel speed state; if the number of the first-screened routes is one, then the route is set as the route for the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the route and the travel speed state; repeat the above steps until the route and travel speed state of the last cycle are obtained, so that the robot moves to the target end point.

[0011] In one embodiment of the present invention, the current speed state of the robot collected, enumerating one or more predicted speed states corresponding to the current cycle includes: based on the set first limit range, calculating the minimum angular velocity and the maximum angular velocity that may be reached in the next cycle according to the current angular velocity; uniformly sampling the angular velocity within the range between the minimum angular velocity and the maximum angular velocity to obtain multiple uniformly sampled angular velocities; calculating the minimum linear velocity and the maximum linear velocity allowed in the next cycle corresponding to each uniformly sampled angular velocity based on each uniformly sampled angular velocity and the current linear velocity; uniformly sampling the linear velocity within the range between the minimum linear velocity and the maximum linear velocity corresponding to each uniformly sampled angular velocity to obtain multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; enumerating multiple predicted speed states based on each uniformly sampled angular velocity and the multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; wherein each predicted speed state includes: a predicted angular velocity and a predicted linear velocity.

[0012] In one embodiment of the present invention, the scoring rules include: one or more of an angle scoring rule, a line speed scoring rule, a target endpoint scoring rule, and a sampling speed scoring rule; wherein the angle scoring rule includes: the smaller the angle between the orientation of the robot at the end point of the initial screening route and the line connecting the current position of the robot and the target endpoint, the higher the score value; the line speed scoring rule includes: the greater the sampling line speed corresponding to the initial screening route, the higher the score value; the target endpoint scoring rule includes: the closer the end point of the robot at the initial screening route is to the target endpoint, the higher the score value; the sampling speed scoring rule includes: the smaller the difference between the uniform sampling speed of the robot and the current speed, the higher the score value.

[0013] To achieve the above-mentioned purpose and other related purposes, the present invention provides a robot dynamic obstacle avoidance system, the system comprising: a global path solving module, used to solve and obtain a global path according to the position information of multiple key points walked by the robot; the global path comprises: a starting point and a target end point; a global path discretization module, connected to the global path solving module, used to discretize the global path, and map the sequence of global path points obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; a local path solving module, connected to the global path discretization module, used to obtain the local path of the robot under each cycle based on the two-dimensional mapping map. A local map mapped by the current position of the robot, a local planning end point in each cycle is selected from the global path points in the local map corresponding to each cycle, and the local path of the robot corresponding to each cycle is obtained based on the local path algorithm; a travel speed route acquisition module is connected to the local path solution module, and is used to enumerate one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route are screened and obtained as the travel route and travel speed of the next cycle, so as to navigate the robot to the end point.

[0014] As described above, the present invention is a robot dynamic obstacle avoidance method and system, which has the following beneficial effects: the present invention provides a robot dynamic obstacle avoidance method and system, which firstly allows the robot to walk through multiple key points that can be selected by the user and obtains a global path based on each key point, and then obtains a local map of each period based on the two-dimensional map mapped by the global path to obtain the local path of the robot corresponding to each period, and obtains the travel route and travel speed in each period based on the local path and the speed state prediction of the robot to perform obstacle avoidance navigation. The present invention can quickly control the robot to avoid obstacles, improve the accuracy of obstacle avoidance, and realize the control of the robot to avoid obstacles safely and efficiently. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 Shown is a schematic flow chart of a robot dynamic obstacle avoidance method in one embodiment of the present invention.

[0016] Figure 2 It is a schematic flow chart showing a method for generating a local path in each cycle in one embodiment of the present invention.

[0017] Figure 3 Shown is a schematic diagram of the structure of a local map in an embodiment of the present invention.

[0018] Figure 4 Shown is a schematic flow chart of a robot dynamic obstacle avoidance method in one embodiment of the present invention.

[0019] Figure 5 Shown is a schematic structural diagram of a robot dynamic obstacle avoidance system in one embodiment of the present invention. DETAILED DESCRIPTION

[0020] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other without conflict.

[0021] It should be noted that in the following description, reference is made to the accompanying drawings, which describe several embodiments of the present invention. It should be understood that other embodiments may also be used, and that mechanical composition, structural, electrical and operational changes may be made without departing from the spirit and scope of the present invention. The following detailed description should not be considered restrictive, and the scope of the embodiments of the present invention is limited only by the claims of the published patents. The terms used herein are only for describing specific embodiments and are not intended to limit the present invention. Spatially related terms, such as "upper", "lower", "left", "right", "below", "below", "lower", "above", "upper", etc., may be used in the text to facilitate the description of the relationship between an element or feature shown in the figure and another element or feature.

[0022] Throughout the specification, when a part is said to be "connected" to another part, this includes not only the case of "direct connection" but also the case of "indirect connection" by placing other elements therebetween. In addition, when a part is said to "include" a certain constituent element, unless otherwise stated, it does not exclude other constituent elements, but means that other constituent elements may be included.

[0023] The terms first, second and third mentioned herein are used to describe various parts, components, regions, layers and / or segments, but are not limited thereto. These terms are only used to distinguish a certain part, component, region, layer or segment from other parts, components, regions, layers or segments. Therefore, the first part, component, region, layer or segment described below may refer to the second part, component, region, layer or segment within the scope of the present invention.

[0024] Furthermore, as used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless there is an indication to the contrary in the context. It should be further understood that the terms "comprise", "include" indicate the presence of the described features, operations, elements, components, items, kinds, and / or groups, but do not exclude the presence, occurrence or addition of one or more other features, operations, elements, components, items, kinds, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or mean any one or any combination. Therefore, "A, B or C" or "A, B and / or C" means "any of the following: A; B; C; A and B; A and C; B and C; A, B and C". Exceptions to this definition will only occur when the combination of elements, functions or operations is inherently mutually exclusive in some way.

[0025] A robot dynamic obstacle avoidance method is provided in an embodiment of the present invention. First, the user can select a plurality of key points to make the robot walk and obtain a global path according to each key point. Then, based on the two-dimensional map mapped by the global path, a local map of each period is obtained to obtain the local path of the robot corresponding to each period. Based on the local path and the speed state prediction of the robot, the travel route and travel speed in each period are obtained to perform obstacle avoidance navigation. The present invention can quickly control the robot to avoid obstacles, improve the accuracy of obstacle avoidance, and realize the control of the robot to avoid obstacles safely and efficiently.

[0026] The following is a detailed description of the embodiments of the present invention with reference to the accompanying drawings so that those skilled in the art can easily implement the present invention. The present invention can be embodied in many different forms and is not limited to the embodiments described herein.

[0027] like Figure 1 As shown, a schematic diagram showing the flow of the robot dynamic obstacle avoidance method in an embodiment of the present invention is shown.

[0028] Applied to a mobile robot, which is provided with a plurality of sensors, the method comprises:

[0029] Step S11: Calculate and obtain the global path based on the position information of multiple key points along which the robot walks.

[0030] Specifically, the global path includes: a starting point and a target end point, wherein the starting point is obtained according to the first key point, and the target end point is obtained according to the last key point.

[0031] Optionally, the user controls the robot to reach multiple key points in sequence, generates a graph structure containing the key points abstracted from the real world, and solves a global path based on the graph structure and using the Dijkstra algorithm, where the starting point is the node on the graph closest to the current position, and the end point is the node on the graph closest to the target. Obstacle avoidance navigation using the global path obtained in this way better meets user needs and has a better obstacle avoidance effect.

[0032] Step S12: discretize the global path, and map the global path point sequence obtained by discretization into a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map.

[0033] Optionally, step S22 includes: discretizing the global path into multiple global path points whose distances to adjacent points are preset distance parameters, and obtaining a global path point sequence; mapping the global path point sequence to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; wherein the two-dimensional mapping map includes: the location information of each obstacle and each global path point in the map coordinates.

[0034] Optionally, the global path point sequence is mapped onto a two-dimensional local grid map generated by fusing information from each sensor to obtain a two-dimensional mapping map.

[0035] Step S13: Based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, the local planning end point in each cycle is selected from the global path points in the local map corresponding to each cycle, and based on the local path algorithm, the local path of the robot corresponding to each cycle is obtained.

[0036] Optionally, we set a fixed time threshold as one cycle, that is, it takes N cycles for the robot to move from the starting point to the end point, where N ≥ 1; the local path planning of the first cycle is performed from the starting point, and the second cycle is performed after the first cycle is completed until the robot moves to the end point. We obtain a total of N cycles of local paths.

[0037] Optional, such as Figure 2 As shown, the local path generation method for each cycle includes:

[0038] Step S21: Based on the two-dimensional mapping map, the current position of the robot is mapped to obtain a local map describing the surrounding environment in the current cycle; the local map includes: one or more global path points and location information of obstacles;

[0039] Step S22: based on the current position and current speed state of the robot, selecting a local planning end point of the current cycle from the global path points in the local map corresponding to the current cycle;

[0040] Step S23: Based on the local path algorithm, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is calculated.

[0041] Optionally, the step S21 includes: when each new cycle starts, the current position of the robot, the position information of some global path points and the position information of some obstacles in the two-dimensional mapping map are converted into a local coordinate system established with the current position of the robot as the center, so as to obtain a local map describing the surrounding environment in the current cycle, so as to perform local planning and tracking of the global path on the local map; it should be noted that the global path points and their number that need to be converted in the current cycle can be selected according to a preset range under the set fixed cycle.

[0042] Optionally, the step S22 includes:

[0043] Based on the current position and current speed state of the robot, a global path point in the corresponding period is selected as the initial local planning end point;

[0044] Based on the obstacle information of the local map in the current cycle, determine whether the initial local planning end point is occupied by an obstacle;

[0045] When the initial local planning endpoint is occupied by an obstacle, the target endpoint judgment process is performed to select a new global path point as the local planning endpoint;

[0046] When the initial local planning endpoint is not occupied by an obstacle, the initial local planning endpoint is used as the local planning endpoint;

[0047] Among them, when the initial local planning end point is occupied by an obstacle, there are two decision-making schemes, that is, the target end point judgment process includes:

[0048] Determine whether the initial local planning endpoint is the target endpoint;

[0049] Decision 1: If it is the target endpoint, based on the target endpoint selection rule, a global path point between the current position of the robot and the initial local planning endpoint and not occupied by an obstacle is selected as the local planning endpoint, that is, a new local endpoint is selected in the direction close to the current position of the robot, which will bring the robot closer to the obstacle.

[0050] Decision 2: If it is not the target endpoint, based on the non-target endpoint selection rule, select a global path point between the initial local planning endpoint and the global path point in the local map that is farthest from the current position of the robot and includes the global path point with the farthest distance from the current position as the local planning endpoint, that is, select a new local endpoint in the direction away from the current position of the robot, which will enable the robot to bypass obstacles.

[0051] Optionally, the target endpoint selection rule includes: selecting the local planning endpoint from the global path points that are located between the current position of the robot and the initial local planning endpoint and are not occupied by obstacles in the order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint; for example, Figure 3 As shown, if the initial local planning end point 3 is occupied by an obstacle and is the target end point, point 2 close to point 3 is first selected according to direction 1. If point 2 is also occupied, point 1 is selected;

[0052] And / or, the non-target endpoint selection rule includes: selecting the global path points between the initial local planning endpoint and the global path point in the local map that is farthest from the current position of the robot and including the global path point that is farthest from the current position as the local planning endpoint in order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint; for example, Figure 3 As shown, if the initial local planning end point 3 is occupied by an obstacle and is not the target end point, point 4 close to point 3 is first selected according to direction 2. If point 4 is also occupied, point 5 is selected. If point 5 is occupied, point 6 is selected.

[0053] Optionally, the step S23 includes:

[0054] Based on the improved Astar core cost function, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is calculated;

[0055] Among them, the improved Astar core cost function adds two weights based on the principle of the Astar core cost function; that is, the improved Astar core cost function includes: the actual distance weight from the starting point of the current cycle to the current position, the distance weight from the current position to the nearest obstacle, the closest distance weight from the current position to the last solved local path, and the estimated value weight from the current position to the local planning end point.

[0056] Optionally, local planning adds two weights based on the principle of the Astar algorithm. The first item increases the distance weight from the current position to the nearest obstacle, that is, the local map generates cost information of 0-255 by expansion. The higher the cost value, the closer the distance to the obstacle. When the cost value is 255, it means that it is impassable. The cost value is included in the path cost, and appropriate parameters are selected through simulation so that the robot will not pass close to the edge of the obstacle when bypassing the obstacle, thereby increasing the safety of travel; the second item increases the distance weight from the current position to the local path solved last time, that is, when an obstacle appears in front, the robot can bypass it on the left and right sides and the costs of the two paths are similar. Due to sensor data errors, travel control errors and other reasons, the robot alternates between the travel strategies on both sides in continuous cycles, causing the robot to swing in place. To solve this problem, the path calculated in the previous cycle is used as a parameter and passed into this calculation process. The distance from the point to the historical path is used as the cost and is weighted to the total path cost, so that when facing a fork in the road decision, it always tends to be inclined to the decision made last time, reducing the problem of swinging in place caused by the previous reasons.

[0057] Optionally, the improved Astar core cost function includes:

[0058] f(s)=k1*g(s)+k2*h(s)+k3*o(s)+k4*r(s); (1)

[0059] Among them, f(s) is the estimated cost from the starting point of the current cycle to the current position and then to the local planning end point, k1*g(s) is the actual distance weight from the starting point of the current cycle to the current position, k2*h(s) is the distance weight from the current position to the nearest obstacle, k3*o(s) is the nearest distance weight from the current position to the local path solved last time, and k4*r(s) is the estimated value weight from the current position to the local planning end point; g(s) is the actual distance value from the starting point of the current cycle to the current position, k1 is the weight coefficient of g(s), h(s) is the distance value from the current position to the nearest obstacle, k2 is the weight coefficient of h(s), o(s) is the nearest distance value from the current position to the local path solved last time, k3 is the weight coefficient of o(s), r(s) is the estimated value from the current position to the local planning end point, and k4 is the weight coefficient of r(s); it should be noted that k1, k2, k3, and k4 can be set according to specific needs.

[0060] The improved Astar core cost function is compared with the unimproved Astar core cost function. With a certain increase in search cost and path length, a path that is farther away from obstacles and can be passed more safely is solved.

[0061] Step S14: Enumerate one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and screen out the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the route and travel speed of the next cycle, so as to navigate the robot to the end point.

[0062] Optionally, step S14 includes:

[0063] Based on the collected current speed state of the robot, enumerate one or more predicted speed states corresponding to the current cycle;

[0064] Based on the set simulation time, each predicted speed state is simulated to obtain one or more initial travel routes;

[0065] Based on the initial screening conditions, one or more initial screening routes are screened from the initial route; optionally, the initial screening conditions include: removing all routes that may encounter obstacles (having obstacles on their paths) and / or removing all routes that cross the desired target point.

[0066] If the number of the initially screened travel routes is at least two, then based on the scoring rules related to the local path, each of the initially screened travel routes is scored, and the screened travel route with the highest score is set as the travel route of the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state;

[0067] If the number of initially screened travel routes is one, then the route is set as the travel route for the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state;

[0068] Repeat the above steps until the travel route and travel speed state of the last cycle are obtained, so that the robot moves to the target end point.

[0069] Optionally, the enumerating one or more predicted speed states corresponding to the current cycle based on the collected current speed state of the robot includes:

[0070] Based on the set first limit range, the minimum angular velocity and the maximum angular velocity that may be reached in the next cycle are calculated according to the current angular velocity; preferably, the first limit range includes: an acceleration limit range or an angular velocity limit range;

[0071] Uniformly sampling the angular velocities within a range between the minimum angular velocity and the maximum angular velocity to obtain a plurality of uniformly sampled angular velocities;

[0072] Based on each uniformly sampled angular velocity and the current linear velocity, the minimum linear velocity and the maximum linear velocity allowed in the next cycle corresponding to each uniformly sampled angular velocity are calculated respectively;

[0073] Uniformly sampling the linear velocities within the range between the minimum linear velocity and the maximum linear velocity corresponding to each uniformly sampled angular velocity, to obtain a plurality of uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity;

[0074] Based on each uniformly sampled angular velocity and a plurality of uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity, a plurality of predicted velocity states (ie, a combination of angular velocity and linear velocity) are enumerated and obtained; wherein each predicted velocity state includes: a predicted angular velocity and a predicted linear velocity.

[0075] Optionally, the scoring rule includes: one or more of an angle scoring rule, a line speed scoring rule, a target endpoint scoring rule, and a sampling speed scoring rule;

[0076] Among them, the angle scoring rule includes: the smaller the angle between the robot's orientation at the end point of the initial screening route and the line connecting the robot's current position and the target end point, the higher the score value; the linear speed scoring rule includes: the greater the sampling linear speed corresponding to the initial screening route, the higher the score; the target end point scoring rule includes: the closer the robot's end point at the initial screening route is to the target end point, the higher the score; the sampling speed scoring rule includes: the smaller the difference between the robot's uniform sampling speed and the current speed, the higher the score.

[0077] The scores of the initially screened routes are normalized, weighted and accumulated to obtain the final score, and the sampling speed with the highest score is selected as the speed instruction for this time. If all paths are filtered, the linear speed and angular speed instructions for this time are both zero.

[0078] Optionally, based on the set simulation time, each predicted speed state is simulated according to an operation formula to obtain one or more initial travel routes (x, y, θ); wherein the operation formula includes:

[0079] x(t+1)=x(t)+v(t)Δt cos(θ(t)); (2)

[0080] y(t+1)=y(t)+v(t)Δt sin(θ(t)); (3)

[0081] θ(t+1)=θ(t)+w(t)Δt; (4)

[0082] In order to better describe the robot dynamic obstacle avoidance method, a specific embodiment is provided.

[0083] Embodiment 1: A robot dynamic obstacle avoidance method. Figure 4 4 is a flow chart of a robot dynamic obstacle avoidance method in one embodiment.

[0084] The method comprises:

[0085] Receive the target point path finding task start; solve the global path according to the position information of multiple key points walked by the robot, discretize the global path, and map the global path point sequence obtained by discretization to the two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map;

[0086] Determining whether the target end point has been reached according to the current position of the robot;

[0087] If yes, adjust the robot to the final orientation and the navigation ends;

[0088] If not, based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, the local map information is updated; based on the local map, an appropriate global path point is selected as the local path end point;

[0089] Determine whether there is a suitable local path endpoint;

[0090] If not, wait where you are;

[0091] If so, the local path is calculated based on the local path algorithm;

[0092] Determine whether the local path exists;

[0093] If not, rotate the robot posture to explore the path;

[0094] If so, determine whether the viewing angle relative to the tracking point is greater than the set angle threshold; if not, proceed according to normal instructions; if so, limit the linear speed, rotate the fuselage to adjust the angular speed for the next navigation cycle until the target end point is reached.

[0095] Similar to the principle of the above embodiment, the present invention provides a robot dynamic obstacle avoidance system.

[0096] The following provides specific embodiments in conjunction with the accompanying drawings:

[0097] like Figure 5 A schematic structural diagram of a robot dynamic obstacle avoidance system in an embodiment of the present invention is shown.

[0098] The system comprises:

[0099] The global path solving module 51 is used to solve and obtain the global path according to the position information of multiple key points of the robot's walking; the global path includes: a starting point and a target end point;

[0100] A global path discretization module 52, connected to the global path solving module 51, is used to discretize the global path and map the global path point sequence obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map;

[0101] A local path solving module 53, connected to the global path discretization module 52, is used to select a local planning end point in each cycle from the global path points in the local map corresponding to each cycle based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, and obtain the local path of the robot corresponding to each cycle based on the local path algorithm;

[0102] The travel speed route acquisition module 54 is connected to the local path solving module 53, and is used to enumerate one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and screen the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the travel route and travel speed of the next cycle, so as to navigate the robot to the end point.

[0103] It should be understood that Figure 3 The division of each module in the system embodiment is only a division of logical functions. In actual implementation, they can be fully or partially integrated into one physical entity, or physically separated. And these modules can be all implemented in the form of software calling through processing elements; or all implemented in the form of hardware; or some modules can be implemented in the form of software calling through processing elements, and some modules can be implemented in the form of hardware;

[0104] For example, each module may be one or more integrated circuits configured to implement the above method, such as: one or more application specific integrated circuits (ASIC), or one or more digital signal processors (DSP), or one or more field programmable gate arrays (FPGA). For another example, when a module above is implemented in the form of a processing element scheduling program code, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processor that can call program code. For another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0105] Therefore, since the implementation principle of the robot dynamic obstacle avoidance system has been described in the above-mentioned embodiment, it will not be repeated here.

[0106] Optionally, the global path discretization module 52 is used to discretize the global path into multiple global path points whose distances to adjacent points are preset distance parameters, and obtain a global path point sequence; map the global path point sequence to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; wherein the two-dimensional mapping map includes: the location information of each obstacle and each global path point in the map coordinates.

[0107] Optionally, the local path solving module 53 is used to map the current position of the robot based on the two-dimensional mapping map to obtain a local map describing the surrounding environment in the current cycle; the local map includes: one or more global path points and location information of obstacles; based on the current position and current speed state of the robot, the local planning end point of the current cycle is selected from the global path points in the local map corresponding to the current cycle; based on the local path algorithm, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is solved.

[0108] Optionally, the local path solving module 53 is used to select a global path point in the corresponding period as the initial local planning end point based on the current position and current speed state of the robot; determine whether the initial local planning end point is occupied by an obstacle; if so, perform a target end point determination process to select a new global path point as the local planning end point; if not, use the initial local planning end point as the local planning end point; wherein the target end point determination process includes: determining whether the initial local planning end point is the target end point; if so, based on the target end point selection rule, select a global path point between the current position of the robot and the initial local planning end point and not occupied by an obstacle as the local planning end point; if not, based on the non-target end point selection rule, select a global path point between the initial local planning end point and the global path point in the local map that is farthest from the current position of the robot and includes the global path point that is farthest from the current position as the local planning end point.

[0109] Optionally, the local path solving module 53 is used to solve the local path of the robot corresponding to the current cycle based on the improved Astar core cost function, according to the current position of the robot and the local planning end point of the current cycle; wherein the improved Astar core cost function includes: the actual distance weight from the starting point of the current cycle to the current position, the distance weight from the current position to the nearest obstacle, the closest distance weight from the current position to the last solved local path, and the estimated value weight from the current position to the local planning end point.

[0110] Optionally, the travel speed route acquisition module 54 is used to enumerate one or more predicted speed states corresponding to the current cycle based on the collected current speed state of the robot; simulate each predicted speed state based on the set simulation time to obtain one or more initial travel routes; based on the initial screening conditions, screen out one or more initial screening travel routes from the initial travel routes; if the number of the initial screening travel routes is at least two, then based on the scoring rules related to the local path, score each initial screening travel route, and make the highest-scoring screening travel route the travel route of the next cycle, and make the predicted speed state of the route the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; if the number of the initial screening travel routes is one, then make the route the travel route of the next cycle, and make the predicted speed state of the route the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; repeat the above steps until the travel route and travel speed state of the last cycle are obtained, so that the robot moves to the target end point.

[0111] Optionally, the travel speed route acquisition module 54 is used to calculate the minimum angular velocity and the maximum angular velocity that may be reached in the next cycle based on the current angular velocity based on the set first limit range; uniformly sample the angular velocities within the range between the minimum angular velocity and the maximum angular velocity to obtain multiple uniformly sampled angular velocities; calculate based on each uniformly sampled angular velocity and the current linear velocity to obtain the minimum linear velocity and the maximum linear velocity allowed in the next cycle corresponding to each uniformly sampled angular velocity; uniformly sample the linear velocity within the range between the minimum linear velocity and the maximum linear velocity corresponding to each uniformly sampled angular velocity to obtain multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; enumerate and obtain multiple predicted speed states based on each uniformly sampled angular velocity and the multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; wherein each predicted speed state includes: a predicted angular velocity and a predicted linear velocity.

[0112] In summary, the robot dynamic obstacle avoidance method and system of the present invention firstly makes the robot walk through multiple key points that can be selected by the user and obtains the global path according to each key point, then obtains the local map of each cycle based on the two-dimensional map mapped by the global path to obtain the local path of the robot corresponding to each cycle, and obtains the travel route and travel speed in each cycle based on the local path and the speed state prediction of the robot to perform obstacle avoidance navigation. The present invention can quickly control the robot to complete obstacle avoidance, improve the obstacle avoidance accuracy, and realize the control of the robot to avoid obstacles safely and efficiently. Therefore, the present invention effectively overcomes the various shortcomings in the prior art and has a high industrial utilization value.

[0113] The above embodiments are merely illustrative of the principles and effects of the present invention, and are not intended to limit the present invention. Anyone familiar with the technology may modify or change the above embodiments without violating the spirit and scope of the present invention. Therefore, all equivalent modifications or changes made by a person of ordinary skill in the art without departing from the spirit and technical ideas disclosed by the present invention shall still be covered by the claims of the present invention.

Claims

1. A robot dynamic obstacle avoidance method, characterized in that: The method comprises: A global path is obtained by calculating the position information of multiple key points of the robot's walking; the global path includes: a starting point and a target end point; Discretizing the global path, and mapping the global path point sequence obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; Based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, the local planning end point in each cycle is selected from the global path points in the local map corresponding to each cycle, and based on the local path algorithm, the local path of the robot corresponding to each cycle is obtained; Enumerate one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and select the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the route and speed of the next cycle, so as to navigate the robot to the end point, which includes: Based on the collected current speed state of the robot, one or more predicted speed states corresponding to the current cycle are enumerated, which include: based on the set first limit range, the minimum angular velocity and the maximum angular velocity that may be reached in the next cycle are calculated according to the current angular velocity; the angular velocity within the range between the minimum angular velocity and the maximum angular velocity is uniformly sampled to obtain multiple uniformly sampled angular velocities; based on each uniformly sampled angular velocity and the current linear velocity, the minimum linear velocity and the maximum linear velocity allowed in the next cycle corresponding to each uniformly sampled angular velocity are calculated respectively; the linear velocity within the range between the minimum linear velocity and the maximum linear velocity corresponding to each uniformly sampled angular velocity is uniformly sampled to obtain multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; based on each uniformly sampled angular velocity and the multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity, multiple predicted speed states are enumerated; wherein each predicted speed state includes: a predicted angular velocity and a predicted linear velocity; Based on the set simulation time, each predicted speed state is simulated to obtain one or more initial travel routes; Based on the initial screening condition, one or more initial screening routes are screened from the initial travel route; If the number of the initially screened travel routes is at least two, then based on the scoring rules related to the local path, each of the initially screened travel routes is scored, and the screened travel route with the highest score is set as the travel route of the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; If the number of initially screened travel routes is one, then the route is set as the travel route for the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; Repeat the above steps until the travel route and travel speed state of the last cycle are obtained, so that the robot moves to the target end point; The scoring rules include: One or more of angle scoring rules, line speed scoring rules, target endpoint scoring rules, and sampling speed scoring rules; The angle scoring rule includes: the smaller the angle between the orientation of the robot at the end point of the initially screened route and the line connecting the current position of the robot and the target end point, the higher the score value; The line speed scoring rule includes: the greater the sampling line speed corresponding to the initially screened travel route, the higher the score; The target endpoint scoring rule includes: the closer the endpoint of the robot at the initial screening route is to the target endpoint, the higher the score; The sampling speed scoring rule includes: the smaller the difference between the average sampling speed of the robot and the current speed, the higher the score.

2. The robot dynamic obstacle avoidance method according to claim 1, characterized in that: The discretizing the global path and mapping the global path point sequence obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map includes: Discretize the global path into a plurality of global path points whose distances from adjacent points are all preset distance parameters, and obtain a sequence of global path points; The global path point sequence is mapped to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; wherein the two-dimensional mapping map includes: position information of each obstacle and each global path point in map coordinates.

3. The robot dynamic obstacle avoidance method according to claim 1, characterized in that: Based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, the local planning end point in each cycle is selected from the global path points in the local map corresponding to each cycle, and based on the local path algorithm, the local path of the robot corresponding to each cycle is obtained. The method includes: Based on the two-dimensional mapping map, the current position of the robot is mapped to obtain a local map describing the surrounding environment in the current cycle; the local map includes: one or more global path points and location information of obstacles; Based on the current position and current speed state of the robot, select a local planning end point of the current cycle from the global path points in the local map corresponding to the current cycle; Based on the local path algorithm, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is calculated.

4. The robot dynamic obstacle avoidance method according to claim 3, characterized in that: The method of selecting the local planning end point of the current cycle from the global path points in the local map corresponding to the current cycle based on the current position and current speed state of the robot includes: Based on the current position and current speed state of the robot, a global path point in the corresponding period is selected as the initial local planning end point; Determine whether the initial local planning endpoint is occupied by an obstacle; If yes, the target endpoint judgment process is performed to select the new global path point as the local planning endpoint; If not, the initial local planning endpoint is used as the local planning endpoint; The target endpoint determination process includes: Determine whether the initial local planning endpoint is the target endpoint; If so, based on the target endpoint selection rule, a global path point between the current position of the robot and the initial local planning endpoint and not occupied by obstacles is selected as the local planning endpoint; If not, based on the non-target endpoint selection rule, a global path point between the initial local planning endpoint and the global path point in the local map that is farthest from the current position of the robot and including the global path point that is farthest from the current position is selected as the local planning endpoint.

5. The robot dynamic obstacle avoidance method according to claim 4, characterized in that: The target endpoint selection rule includes: selecting a local planning endpoint from the global path points that are located between the current position of the robot and the initial local planning endpoint and are not occupied by obstacles in the order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint; And / or, the non-target endpoint selection rule includes: selecting the global path points between the initial local planning endpoint and the global path point in the local map that is farthest from the current position of the robot and including the global path point that is farthest from the current position as the local planning endpoint in order from the closest to the initial local planning endpoint to the farthest from the initial local planning endpoint.

6. The robot dynamic obstacle avoidance method according to claim 3, characterized in that: The method of calculating and obtaining the local path of the robot corresponding to the current cycle based on the local path algorithm according to the current position of the robot and the local planning end point of the current cycle includes: Based on the improved Astar core cost function, according to the current position of the robot and the local planning end point of the current cycle, the local path of the robot corresponding to the current cycle is calculated; The improved Astar core cost function includes: the actual distance weight from the starting point of the current cycle to the current position, the distance weight from the current position to the nearest obstacle, the closest distance weight from the current position to the last solved local path, and the estimated value weight from the current position to the local planning end point.

7. A robot dynamic obstacle avoidance system, characterized in that: The system comprises: A global path solving module is used to solve and obtain a global path according to the position information of multiple key points where the robot walks; the global path includes: a starting point and a target end point; A global path discretization module, connected to the global path solving module, is used to discretize the global path and map the global path point sequence obtained by discretization to a two-dimensional map obtained based on multi-sensor fusion to obtain a two-dimensional mapping map; A local path solving module, connected to the global path discretization module, is used to select a local planning end point in each cycle from the global path points in the local map corresponding to each cycle based on the local map mapped by the current position of the robot in each cycle obtained by the two-dimensional mapping map, and obtain the local path of the robot corresponding to each cycle based on the local path algorithm; The travel speed route acquisition module is connected to the local path solving module, and is used to enumerate one or more predicted speed states corresponding to each cycle and the predicted routes corresponding to each predicted speed state in each cycle, and screen and obtain the predicted route that best matches the corresponding local path in each cycle and the predicted speed state corresponding to the route as the travel route and travel speed of the next cycle, so as to navigate the robot to the end point, which includes: Based on the collected current speed state of the robot, one or more predicted speed states corresponding to the current cycle are enumerated, which include: based on the set first limit range, the minimum angular velocity and the maximum angular velocity that may be reached in the next cycle are calculated according to the current angular velocity; the angular velocity within the range between the minimum angular velocity and the maximum angular velocity is uniformly sampled to obtain multiple uniformly sampled angular velocities; based on each uniformly sampled angular velocity and the current linear velocity, the minimum linear velocity and the maximum linear velocity allowed in the next cycle corresponding to each uniformly sampled angular velocity are calculated respectively; the linear velocity within the range between the minimum linear velocity and the maximum linear velocity corresponding to each uniformly sampled angular velocity is uniformly sampled to obtain multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity; based on each uniformly sampled angular velocity and the multiple uniformly sampled linear velocities corresponding to each uniformly sampled angular velocity, multiple predicted speed states are enumerated; wherein each predicted speed state includes: a predicted angular velocity and a predicted linear velocity; Based on the set simulation time, each predicted speed state is simulated to obtain one or more initial travel routes; Based on the initial screening condition, one or more initial screening routes are screened from the initial travel route; If the number of the initially screened travel routes is at least two, then based on the scoring rules related to the local path, each of the initially screened travel routes is scored, and the screened travel route with the highest score is set as the travel route of the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; If the number of initially screened travel routes is one, then the route is set as the travel route for the next cycle, and the predicted speed state of the route is set as the travel speed state, so that the robot navigates the next cycle with the travel route and the travel speed state; Repeat the above steps until the travel route and travel speed state of the last cycle are obtained, so that the robot moves to the target end point; The scoring rules include: One or more of angle scoring rules, line speed scoring rules, target endpoint scoring rules, and sampling speed scoring rules; The angle scoring rule includes: the smaller the angle between the orientation of the robot at the end point of the initially screened route and the line connecting the current position of the robot and the target end point, the higher the score value; The line speed scoring rule includes: the greater the sampling line speed corresponding to the initially screened travel route, the higher the score; The target endpoint scoring rule includes: the closer the endpoint of the robot at the initial screening route is to the target endpoint, the higher the score; The sampling speed scoring rule includes: the smaller the difference between the average sampling speed of the robot and the current speed, the higher the score.

Images