CN112254727A

CN112254727A - TEB-based path planning method and device

Info

Publication number: CN112254727A
Application number: CN202011006826.0A
Authority: CN
Inventors: 胡朝红
Original assignee: Ruijie Networks Co Ltd
Current assignee: Ruijie Networks Co Ltd
Priority date: 2020-09-23
Filing date: 2020-09-23
Publication date: 2021-01-22
Anticipated expiration: 2040-09-23
Also published as: CN112254727B

Abstract

The invention discloses a path planning method and a device based on a time elastic band TEB, wherein the method is suitable for local path planning of a mobile robot and comprises the following steps: determining a local initial path of the local path planning according to a preset local initial path selection condition; and according to the local initial path, carrying out TEB optimization on the local initial path according to a preset self-adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path. The embodiment of the invention can solve the problems of low stability, low accuracy and poor real-time performance of path planning in the prior art.

Description

TEB-based path planning method and device

Technical Field

The invention relates to the technical field of electronic robots, in particular to a Time Elastic Band (TEB) based path planning method and device.

Background

The mobile robot is a robot system composed of a sensor, a remote controller and an automatically controlled mobile carrier, and integrates latest research results of multiple subjects such as machinery, electronics, computers, automatic control, artificial intelligence and the like. The mobile robot can be applied to various fields such as industry, service industry, agriculture and the like, and can replace manual work to execute repeated and dangerous work.

In a mobile robot capable of automatic path planning, a navigation technique is a core. Generally, a mobile robot first uses a sensor (laser or camera) and a corresponding mapping technology to build a map, which represents static obstacles in the environment, and also often encounters dynamic obstacles, such as moving people, in the environment in which the robot actually operates. The key of the navigation technology of automatic path planning is to combine the known static obstacles and the real-time obstacle information obtained by sensing through the sensor to plan a path from the current position to the designated end point, and simultaneously, the planned path should be executable and smooth to ensure that the robot avoids the obstacles.

In recent years, much attention has been paid to the research of automatic path planning technology, and particularly, there are some mature navigation frameworks and automatic path planning technologies on a Robot Operating System (ROS), in which a Dynamic Window Approach (DWA) and a Time Elastic Band (TEB) technology are two typical technologies.

The existing path planning method framework generally comprises two steps: the first step is global path planning, namely, a global path from a starting point to an end point is planned on a map built in advance, the second step is local path planning, namely, a local path is planned in a local range of the current position of the robot under the guidance of the global path, and finally, a speed command which can be executed by a motor driver is generated so as to control the movement of the robot. Global path planning is generally less frequent because the distance between the start point and the end point may be long and the amount of computation is large. The frequency of the local path planning is higher, and is generally above 10HZ, so as to ensure the continuity of the control. Algorithms for global path planning generally adopt algorithms such as A, D, Dijkstra and the like, or even global paths are manually specified and automatic planning is not needed.

Local path planning algorithms typically use TEB algorithms; the TEB algorithm is a nonlinear optimization algorithm, and firstly a global path is used as an initial track to obtain a series of discrete robot poses (Pose) in a local range and time difference (TimeDiff) variables corresponding to the poses. The pose and time difference on the track need to meet the constraints of speed constraint, acceleration constraint, robot kinematics constraint, distance constraint between obstacles and the like, the constraints can be modeled into a nonlinear optimization problem, so that the optimized track is obtained, the speed can be calculated through the pose and time difference, and the speed can be issued to the robot for control. The track can be considered in the environment beyond a few meters, so that the obstacle in a far distance can be avoided in time, and better motion control can be obtained.

However, the disadvantage of the TEB algorithm is that the trajectory optimization is very dependent on the initial trajectory, and if the initial trajectory cannot effectively avoid the obstacle, then the TEB does not yield good results no matter how optimized it is. The initial trajectory depends on the global path, and the updating frequency of the global path is low, which means that the optimization result of the TEB cannot be guaranteed, thereby causing the problems of low stability, low accuracy and poor instantaneity of path planning.

Disclosure of Invention

The embodiment of the invention provides a path planning method and device based on a time elastic band TEB, which are used for solving the problems of low stability, low accuracy and poor real-time performance of path planning in the prior art.

The embodiment of the invention provides a path planning method based on a time elastic band TEB on the one hand, which is suitable for local path planning of a mobile robot and comprises the following steps:

determining a local initial path of the local path planning according to a preset local initial path selection condition;

and according to the local initial path, carrying out TEB optimization on the local initial path according to a preset self-adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path.

Preferably, the determining the local initial path of the local path planning according to the preset local initial path selection condition includes:

if the TEB optimization continuous failure times are larger than the preset times; or the like, or, alternatively,

if the path oscillation occurs, and the duration time of the path oscillation exceeds a preset time threshold; or the like, or, alternatively,

if the offset of the starting point and the end point of the local path planning is larger than the preset offset threshold compared with the starting point and the end point of the local path planning at the last time,

carrying out global path planning according to the local cost map to obtain a local initial path of the local path planning; the starting point of the local path planning is a position point of the mobile robot, and the end point of the local path planning is a point of a global path at the edge of the local cost map; the local cost map is a map corresponding to an area with the mobile robot as the center and the area as the preset area;

otherwise, the local optimized path obtained last time is used as the local initial path of the local path planning.

Preferably, the global path planning according to the current local cost map includes:

and taking the local cost map as a global cost map, planning a global path by adopting a Dijkstra algorithm, and taking the obtained global path as a local initial path.

Preferably, the method further comprises:

determining whether the obstacles in the local cost map exist in the local cost map at the last time;

if yes, judging whether the obstacles are respectively positioned on the left side of the local initial path of the time and on the right side of the local initial path of the last time or positioned on the right side of the local initial path of the time and on the left side of the local initial path of the last time, and if yes, determining that path oscillation occurs.

Preferably, the performing TEB optimization on the local initial path according to a preset adaptive obstacle avoidance distance includes:

the following operations are carried out on each pose on the local initial path:

determining the distance between the left and right obstacles closest to the current pose to obtain a first distance and a second distance;

and calculating an average value of the first distance and the second distance, and adjusting the pose until the average value is greater than or equal to a preset minimum obstacle avoidance distance and less than or equal to a preset maximum obstacle avoidance distance.

On the other hand, the embodiment of the present invention further provides a path planning device based on a time elastic band TEB, the device is suitable for local path planning of a mobile robot, and the device includes: a determining unit and an optimizing unit; wherein the content of the first and second substances,

the determining unit is used for determining a local initial path of the local path planning according to a preset local initial path selection condition;

and the optimization unit is used for carrying out TEB optimization on the local initial path according to the local initial path and a preset self-adaptive obstacle avoidance distance so as to obtain a successfully optimized local optimized path.

Preferably, the determining unit determines the local initial path of the local path planning according to a preset local initial path selection condition, and is specifically configured to:

Preferably, the determining unit performs global path planning according to the local cost map of this time, and is specifically configured to:

Preferably, the determining unit is further configured to:

Preferably, the optimization unit performs TEB optimization on the local initial path according to a preset adaptive obstacle avoidance distance, and is specifically configured to:

The invention has the following beneficial effects:

the path planning method and device based on the time elastic band TEB are suitable for local path planning of a mobile robot, and a local initial path of the local path planning is determined according to preset local initial path selection conditions; and according to the local initial path, carrying out TEB optimization on the local initial path according to a preset self-adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path. According to the TEB-based path planning method and device provided by the embodiment of the invention, when local path planning is carried out each time, the local initial path needs to be determined, the accuracy of the TEB optimized local path can be ensured, the calculated amount of the local initial path is smaller than that of a global path, the real-time performance is ensured, path optimization can be carried out according to the preset self-adaptive obstacle avoidance distance, the obstacle avoidance distance of the path can be flexibly adjusted even if a narrow channel is touched, and the stability and reliability of the path are improved.

Drawings

FIG. 1 is a flow chart of a TEB-based path planning method in an embodiment of the present invention;

FIG. 2 is a diagram illustrating a global path and a local initial path according to an embodiment of the present invention;

FIG. 3 is a schematic diagram of path planning during path oscillation according to an embodiment of the present invention;

FIG. 4 is a diagram illustrating an embodiment of determining a path oscillation;

fig. 5 is a schematic structural diagram of a TEB-based path planning apparatus according to an embodiment of the present invention.

Detailed Description

Aiming at the problems of low stability, low accuracy and poor real-time performance of path planning in the prior art, the method for planning the path based on the TEB provided by the embodiment of the invention determines the local initial path before local path planning each time, ensures that the local path is optimized on the basis of the relatively accurate initial path, and further ensures the accuracy and stability of the optimized path. The flow of the method of the invention is shown in figure 1, and the execution steps are as follows:

step 101, determining a local initial path of the local path planning according to a preset local initial path selection condition;

in the path planning of the mobile robot, a cost map (Costmap) is a rasterized obstacle map and records static obstacle and dynamic obstacle information in the map; the dynamic obstacle information is acquired in real time by a sensor of the mobile robot, such as a laser radar or a 3D camera. The Global cost map (Global Costmap) is a Global obstacle map and is used for Global path planning; a Local cost map (Local Costmap) is a Local map near a mobile robot, and the size of the map is usually the size of a range that the mobile robot can sense, and is used for planning a Local path.

The global path planning is planned every n seconds, and n is 5-10 in a common situation. The mainstream algorithms for global path planning include algorithms a, D, Dijkstra, and the like. In the embodiment of the present invention, the Dijkstra algorithm is preferably used, and although the Dijkstra algorithm has a larger calculation amount than a and D, the Dijkstra algorithm can relatively stably provide an optimal global path, and provides a good initial value for the next local path planning.

The embodiment of the invention adds a layer of local initial path planning on the basis of the main stream navigation frame, namely, the local initial path is determined once during each local path planning, so that each local path planning is carried out on the basis of a more accurate local initial path.

And 102, according to the local initial path, carrying out TEB optimization on the local initial path according to a preset self-adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path.

In order to avoid obstacles in the TEB optimization process, obstacle constraint conditions are increased. The Pose Pose on the path is required to have a certain distance dist from the nearest barrier PO by barrier constraint, in the existing TEB optimization, dist is a fixed value, if the parameter is set to be too small, the optimized track is very close to the barrier, the requirement of a mobile robot cannot be well met under the condition, if the parameter is set to be too large, the optimization can be in an unstable state when the robot passes through a narrow channel, and the obstacle avoidance constraint cannot be met, so that the robot cannot pass through the narrow channel. Aiming at the problem, the embodiment of the invention adopts the preset self-adaptive obstacle avoidance distance to carry out TEB optimization on the local initial path, and the preset self-adaptive obstacle avoidance distance can be adjusted according to the distance of the obstacle so as to meet the obstacle avoidance constraint condition.

Preferably, in step 101, the determining the local initial path of the local path planning according to the preset local initial path selection condition includes:

if the TEB optimization continuous failure times are larger than the preset times; here, the preset number of times may be 1 time, 2 times, etc.; or the like, or, alternatively,

here, the duration of the path oscillation may be determined by setting two timers T0 and T1, which are specifically: initializing T0, detecting path oscillation each time, updating T0 to be the current time if no path oscillation is determined, and updating T1 to be the current time if the path oscillation occurs, wherein T1-T0 is the duration of the path oscillation; if the path oscillates all the time, T0 does not change, and T1 increases until T1-T0 exceed a preset time threshold, which may be set according to requirements, such as 1 second, 2 seconds, and the like;

if the offset of the starting point and the end point of the local path planning is greater than the preset offset threshold compared with the starting point and the end point of the local path planning at the last time, here, the preset offset threshold may be 10 meters, 15 meters, and the like;

otherwise, if none of the three conditions is met, the local optimized path obtained by the last local optimization is more accurate, and the local optimized path obtained by the last local optimization can be directly used as the local initial path of the local path planning to perform the path optimization.

The global path planning according to the local cost map of this time includes:

Here, the Local initial path planning may also be performed by using Dijkstra algorithm, which performs planning on Local Costmap. The end point of the Local initial path plan is selected as the point on the global path that falls at the edge of the Local Costmap, as shown in FIG. 2. The solid line in fig. 2 is a planned global path, and a represents an obstacle, and the path may collide with the obstacle due to the low planning frequency of the global path. The local initial path is planned in each control period, so that the obstacles can be avoided in time. And after the local initial path is generated, the local initial path is sent to a TEB for optimization, and finally the TEB generates a speed command according to the optimized track to control the robot to move. Note that in the present solution, initial path planning is performed in each control cycle, and since the Local initial path planning is performed on the Local Costmap, the calculation amount is small, and the real-time performance can be ensured. Alternatively, the initial path planning may be performed every several control cycles.

Preferably, the method further comprises:

Here, the local initial path planning has a problem: path oscillation problem. As shown in fig. 3, when there is an obstacle in front of the mobile robot, the local path planning will plan two alternative paths: an a path and a B path. Because the local initial path planning frequency is high, the path A is planned at the time t, and the path B may be planned at the time t +1, which is the path oscillation. Such path oscillations occur as long as the planning frequency is high, but global path planning generally does not have such a high planning frequency and therefore occurs less frequently. The path oscillation may cause the robot control to sway.

The essence of the path oscillation is that the planned path diverges due to the presence of obstacles, and is therefore examined around the obstacles. If an obstacle is located to the left of the path in the previous control period and to the right of the path in the next control period, the path plan is considered to oscillate. Suppose that the path a is the path planned in the last control cycle and the path B is the path planned at this moment. There are many obstacles in Costmap, and it is first necessary to identify which obstacles were present in the last planning because path oscillations do not occur around the new obstacle.

In the embodiment of the present invention, determining whether an obstacle in the current local cost map exists in the previous local cost map may specifically be:

if the position of the obstacle in the path planning is similar to that of the obstacle in the last planning, and the obstacle is within the preset threshold range, the obstacle is considered to exist in the last planning.

Then, poses Pa0, Pa1, Pa2 and Pb0, Pb1, Pb2 can be obtained by sampling the a path and the B path 3 times respectively, the positions of the sampling being at paths 1/4, 1/2, 3/4, the orientations of the poses being estimated from the front and rear adjacent positions on the paths, as shown in fig. 4, where PO is an obstacle. With a simple trigonometric function, it can be found whether the PO is to the left or right of Pa 0. If P0 is located to the right of Pa0 and at the same time to the left of Pb0, it indicates that a path oscillation has occurred. In the embodiment of the present invention, three sets of data (Pa0, Pb0, PO), (Pa1, Pb1, PO), (Pa2, Pb2, PO) may be checked separately, and as long as there is one set of the data that checks oscillation, it may be determined that oscillation occurs in the path.

Optionally, the performing TEB optimization on the local initial path according to a preset adaptive obstacle avoidance distance includes:

Wherein the pose comprises a position and an orientation; the orientation can be obtained according to the path after the last TEB optimization, and can also be determined according to the connecting line of the front and back adjacent two points; typically, the path is determined by discrete points, which are positions of the pose.

Specifically, a minimum obstacle avoidance distance min _ dist and a maximum obstacle avoidance distance max _ dist are preset, and each position on the track checks the distance between the left obstacle and the right obstacle which are closest to the position, so as to obtain a first distance left _ obs _ dist and a second distance right _ obs _ dist. The adaptive obstacle avoidance distance during optimization can be dist ═ left _ obs _ dist + right _ obs _ dist)/2, and min _ dist ≦ max _ dist.

The path planning method based on the time elastic band TEB is suitable for local path planning of a mobile robot, and a local initial path of the local path planning is determined according to a preset local initial path selection condition; and according to the local initial path, carrying out TEB optimization on the local initial path according to a preset self-adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path. According to the TEB-based path planning method and device provided by the embodiment of the invention, when local path planning is carried out each time, the local initial path needs to be determined, the accuracy of the TEB optimized local path can be ensured, the calculated amount of the local initial path is smaller than that of a global path, the real-time performance is ensured, path optimization can be carried out according to the preset self-adaptive obstacle avoidance distance, the obstacle avoidance distance of the path can be flexibly adjusted even if a narrow channel is touched, and the stability and reliability of the path are improved.

Based on the same inventive concept, an embodiment of the present invention provides a path planning apparatus based on TEB, which may be applied to local path planning of a mobile robot, and the structure of the apparatus is as shown in fig. 5, including: a determination unit 51 and an optimization unit 52; wherein the content of the first and second substances,

the determining unit 51 is configured to determine a local initial path of the local path planning according to a preset local initial path selection condition;

the optimizing unit 52 is configured to perform TEB optimization on the local initial path according to the local initial path and a preset adaptive obstacle avoidance distance to obtain a successfully optimized local optimized path.

The determining unit 51 determines the local initial path of the local path planning according to a preset local initial path selection condition, and is specifically configured to:

The determining unit 51 performs global path planning according to the local cost map of this time, and is specifically configured to:

Further, the determining unit 51 is further configured to:

The optimization unit 52 performs TEB optimization on the local initial path according to a preset adaptive obstacle avoidance distance, and is specifically configured to:

It should be understood that the implementation principle and process of the TEB-based path planning apparatus provided in the embodiment of the present invention are similar to those in the embodiments shown in fig. 1 to 4, and are not described herein again.

Those of ordinary skill in the art will understand that: the figures are merely schematic representations of one embodiment, and the blocks or flow diagrams in the figures are not necessarily required to practice the present invention.

From the above description of the embodiments, it is clear to those skilled in the art that the present invention can be implemented by software plus necessary general hardware platform. Based on such understanding, the technical solutions of the present invention may be embodied in the form of a software product, which may be stored in a storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method according to the embodiments or some parts of the embodiments.

The embodiments in the present specification are described in a progressive manner, and the same and similar parts among the embodiments are referred to each other, and each embodiment focuses on the differences from the other embodiments. In particular, for apparatus or system embodiments, since they are substantially similar to method embodiments, they are described in relative terms, as long as they are described in partial descriptions of method embodiments. The above-described embodiments of the apparatus and system are merely illustrative, and the units described as separate parts may or may not be physically separate, and the parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.

In addition, in some of the flows described in the above embodiments and the drawings, a plurality of operations are included in a specific order, but it should be clearly understood that the operations may be executed out of the order presented herein or in parallel, and the sequence numbers of the operations, such as 201, 202, 203, etc., are merely used for distinguishing different operations, and the sequence numbers themselves do not represent any execution order. Additionally, the flows may include more or fewer operations, and the operations may be performed sequentially or in parallel. It should be noted that, the descriptions of "first", "second", etc. in this document are used for distinguishing different messages, devices, modules, etc., and do not represent a sequential order, nor limit the types of "first" and "second" to be different.

The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.

The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

While alternative embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. It is therefore intended that the following appended claims be interpreted as including alternative embodiments and all such alterations and modifications as fall within the scope of the invention.

It will be apparent to those skilled in the art that various modifications and variations can be made in the embodiments of the present invention without departing from the spirit or scope of the embodiments of the invention. Thus, if such modifications and variations of the embodiments of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to encompass such modifications and variations.

Claims

1. A method for path planning based on a Time Elastic Band (TEB), which is suitable for local path planning of a mobile robot, and is characterized by comprising the following steps:

2. The method according to claim 1, wherein the determining the local initial path of the local path planning according to the preset local initial path selection condition comprises:

3. The method according to claim 2, wherein the global path planning according to the current local cost map comprises:

4. The method of claim 2, further comprising:

5. The method of claim 1, wherein the TEB optimizing the local initial path according to a preset adaptive obstacle avoidance distance comprises:

6. A path planning apparatus based on a time elastic band TEB, the apparatus being adapted for local path planning of a mobile robot, comprising: a determining unit and an optimizing unit; wherein the content of the first and second substances,

7. The apparatus according to claim 6, wherein the determining unit determines the local initial path of the local path planning according to a preset local initial path selection condition, and is specifically configured to:

8. The apparatus according to claim 7, wherein the determining unit performs global path planning according to the current local cost map, and is specifically configured to:

9. The apparatus of claim 7, wherein the determining unit is further configured to:

10. The apparatus of claim 6, wherein the optimization unit performs TEB optimization on the local initial path according to a preset adaptive obstacle avoidance distance, and is specifically configured to: