CN114415665A

CN114415665A - Algorithm for obstacle avoidance path planning

Info

Publication number: CN114415665A
Application number: CN202111549089.3A
Authority: CN
Inventors: 周童
Original assignee: Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd
Current assignee: Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd
Priority date: 2021-12-17
Filing date: 2021-12-17
Publication date: 2022-04-29

Abstract

The invention discloses an algorithm for obstacle avoidance path planning, which is characterized in that a bounding box technology and equivalent expansion are adopted, the obstacles and the robot are simplified, a path point set is generated, a corresponding path section is generated, the path section and the bounding box are subjected to interference detection, if interference exists, a new path point is obtained through the bounding box, a new path section is generated until the path section does not interfere with the obstacles, and the calculation amount of the algorithm is less.

Description

Algorithm for obstacle avoidance path planning

[ technical field ] A method for producing a semiconductor device

The invention relates to an algorithm for obstacle avoidance path planning, and belongs to the technical field of path planning algorithms.

[ background of the invention ]

Path planning is an important link in the field of robots, and aims to find a path from a starting point to an end point without collision and meeting other constraints under the condition of obstacles.

Path planning algorithms widely used today are: d, a, fast random tree (RRT) and its variants, Probabilistic Roadmap (PRM) and its variants, genetic algorithms, ant colony algorithms, artificial potential field methods, etc., but the above path planning algorithms have a large amount of operations or a long search time.

[ summary of the invention ]

The technical problem to be solved by the invention is to provide an algorithm for obstacle avoidance path planning, which has less calculation amount.

The technical scheme adopted by the invention is as follows:

an algorithm for obstacle avoidance path planning comprises a robot and an obstacle, wherein the space where the robot and the obstacle are located is a task space, and the algorithm comprises the following steps:

s1, acquiring and inputting information: acquiring and inputting a starting point coordinate and an end point coordinate of robot motion and coordinates of vertexes of all obstacles;

s2, simplified obstacle and robot: simplifying the robot into points, constructing bounding boxes for each obstacle, and expanding the bounding boxes, wherein the expanded width of each bounding box is equal to the radius of each point;

s3, generating a path point set and an initial path: generating a path point set which is a path point set finally used for forming a path, adding a starting point coordinate and an end point coordinate into the path point set, and generating a path section according to the path points in the path point set;

s4, interference detection: performing interference detection on the path segment and the bounding box;

s5, path re-planning: when the path segment interferes with the bounding box, the straight line of the path segment divides the bounding box into two parts, the Euclidean distance sum from the top point contained in the two parts to the straight line of the path segment is respectively calculated, the top point contained in the part with smaller Euclidean distance sum is selected as a candidate path point, and the candidate path point and the top point are sequentially added in front of the terminal point coordinate from small to large according to the distance from the candidate path point to the starting point;

s6, connecting the newly added path points with other points except the adjacent point, judging whether the new path segment interferes with the bounding box, if so, abandoning the connection, otherwise, taking the connection as the new path segment, removing other path points between the two points, and updating the path; directly connecting the end point with other points except the adjacent point in the path point set, judging whether a new path section interferes with the bounding box, if so, abandoning the connection, otherwise, taking the connection as a new path section, and removing other path points between the two points;

s8, judging whether all paths interfere with the bounding box or not, if so, repeating the step 5; otherwise, generating a collision-free path;

s9, path smoothing: and performing smoothing processing on the finally generated collision-free path.

The beneficial effects of the invention are as follows:

according to the invention, the barrier and the robot are simplified by adopting a bounding box technology and equivalent expansion, the calculated amount is greatly reduced, path points are provided for a reconstructed path, and the path of the robot is reconstructed by adopting a vertex, so that the whole calculation process is simpler and more convenient, and the final produced path is as short as possible, thereby greatly shortening the path traveling time of the robot, and simultaneously reducing the planning time.

Preferably, the bounding box is an AABB bounding box or an OBB bounding box.

Preferably, in S2, the task space is divided into a plurality of areas, the bounding boxes are numbered, and the areas in which the bounding boxes are located are recorded.

Preferably, when the movement routes of the robots are on the same plane, the task space is divided into 4 partitions; when the action route of the robot is three-dimensional, the task space is divided into 8 partitions.

Preferably, in S4, the partition in which the path segment is located is recorded, and the bounding box in the partition and the path segment are subjected to interference detection.

Preferably, in S5, when the path segment interferes with the bounding box, the coordinates of interference points of the path segment and the bounding box are calculated, euclidean distances between the coordinates of the interference points and the last point in the set of path points except the end point are calculated, and the interference point corresponding to the minimum distance is selected.

Preferably, when there is an overlap between the bounding boxes, the overlapping bounding boxes are treated as one bounding box by performing a boolean operation of adding the overlapping bounding boxes.

Preferably, when the start point or the end point is within the range of the bounding box, the vertex of the obstacle or the vertex of the bounding box is used as the new start point or end point.

Preferably, when the starting point or the end point is located within the range of the bounding box, the straight line where the path segment is located divides the bounding box into two parts, sums of euclidean distances from vertexes of the bounding box included in the two parts to the straight line where the path segment is located are calculated respectively, vertexes of the bounding box included in the part with the smaller sum of the euclidean distances and vertices of obstacles on the edge of the bounding box are selected as candidate path points, the candidate path points and the starting point or the end point are connected into line segments respectively and interference detection is performed on the obstacles in the bounding box, a candidate path point P with a detection result of no interference is selected as a candidate escape point, a candidate escape point with the smaller euclidean distance from the end point or the starting point is selected as an escape point, and the escape point is used as a new starting point or end point.

Other features and advantages of the present invention will be disclosed in more detail in the following detailed description of the invention and the accompanying drawings.

[ description of the drawings ]

The invention is further described below with reference to the accompanying drawings:

FIG. 1 is a simplified flow diagram of an embodiment of the present invention;

FIG. 2 is a first simulation diagram according to an embodiment of the present invention;

FIG. 3 is a second simulation diagram of the embodiment of the present invention;

FIG. 4 is a third simulation diagram of the embodiment of the present invention;

FIG. 5 is a fourth simulation diagram of the embodiment of the present invention;

FIG. 6 is a fifth simulation of the present invention;

FIG. 7 is a sixth simulation of an embodiment of the present invention;

FIG. 8 is a seventh simulation diagram according to an embodiment of the present invention;

FIG. 9 is a diagram illustrating an eighth simulation according to an embodiment of the present invention;

FIG. 10 is a diagram illustrating a simulation of an embodiment of the present invention;

fig. 11 is a simulation diagram ten according to the embodiment of the present invention.

[ detailed description ] embodiments

The technical solutions of the embodiments of the present invention are explained and illustrated below with reference to the drawings of the embodiments of the present invention, but the following embodiments are only preferred embodiments of the present invention, and not all embodiments. Based on the embodiments in the implementation, other embodiments obtained by those skilled in the art without any creative effort belong to the protection scope of the present invention.

In the description of the present invention, it is to be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "clockwise", "counterclockwise", and the like, indicate orientations and positional relationships based on those shown in the drawings, and are used only for convenience of description and simplicity of description, and do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus, are not to be considered as limiting the present invention.

Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the present invention, "a plurality" means two or more unless explicitly defined otherwise.

In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "secured," and the like are to be construed broadly and can, for example, be fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.

As shown in fig. 1 to 11, the present embodiment shows an algorithm for obstacle avoidance path planning, which includes a robot and an obstacle, where a space where the robot and the obstacle are located is a task space, and the algorithm includes the following steps:

s6, path optimization: connecting the newly added path points with other points except the adjacent point in the path point set, judging whether a new path section interferes with the bounding box, if so, giving up the connection, otherwise, taking the connection as a new path section, removing other path points between the two points, and updating the path; directly connecting the end point with other points except the adjacent point in the path point set, judging whether a new path section interferes with the bounding box, if so, abandoning the connection, otherwise, taking the connection as a new path section, and removing other path points between the two points;

In this embodiment, the task space is divided into a plurality of regions in the S2, the bounding boxes are numbered and the regions where the bounding boxes are located are recorded, and when the movement routes of the robot are on the same plane, the task space is divided into 4 sub-regions; when the movement route of the robot is three-dimensional, the task space is divided into 8 sections, the section in which the path segment is located is recorded in S4, and the bounding box in the section and the path segment are subjected to interference detection.

In this embodiment, in S5, when the path segment interferes with the bounding box, the coordinates of the interference points of the path segment and the bounding box are calculated, the euclidean distances between the coordinates of the interference points and the last point in the set of path points except the end point are respectively calculated, and the interference point corresponding to the minimum distance is selected.

When the bounding boxes are overlapped, the overlapped bounding boxes are treated as one bounding box through the Boolean operation of addition, when the starting point or the end point is positioned in the range of the bounding box, the straight line of the path segment divides the bounding box into two parts, the sum of Euclidean distances from the top point of the bounding box contained in the two parts to the straight line of the path segment is respectively calculated, selecting a bounding box vertex contained in a part with smaller Euclidean distance sum and an obstacle vertex on the edge of the bounding box as candidate path points, respectively connecting the candidate path points with a starting point or an end point to form a line segment, carrying out interference detection on the obstacle in the bounding box, taking a candidate path point P with a detection result of no interference as a candidate escape point, and selecting the candidate escape points with smaller Euclidean distance from the end point or the starting point as escape points, and taking the escape points as new starting points or end points.

As shown in fig. 2 to 11, in this embodiment, a two-dimensional task space of 9 × 11 is taken as an example, 5 obstacles with different shapes exist in the task space, coordinates of a start point of the robot are (1,1), coordinates of an end point of the robot are (9,8), and a path planning process of this embodiment is as follows:

step 1: as shown in fig. 2, information is acquired and input, the coordinates of the starting point of the robot are (1,1), the coordinates of the ending point of the robot are (9,8), and the coordinates of the vertexes of 5 obstacles are also known;

step 2: as shown in fig. 3, for each barrier member AABB bounding box, it can be understood that the AABB bounding box adopted in the present embodiment simplifies the barrier, for example, for obsacle-1, find the maximum, minimum values Xmax, Xmi, Ymax, Ymi of all vertices in the X, Y direction, and combine X, Y values two by two as the vertices of the bounding box, although in other embodiments, OBB bounding boxes can be adopted to simplify the barrier;

and step 3: as shown in fig. 4, the robot is simplified to a point, and the radius of the point is equivalent to the expansion width of all bounding boxes, in this embodiment, if the radius of the simplified point of the robot is 0.2, the width of the bounding box is equivalent to the expansion width of 0.2;

and 4, step 4: as shown in fig. 5, in the present embodiment, the task space is equally divided into 4 partitions, where the upper right partition is a partition 1, the upper left partition is a partition 2, the lower left partition is a partition 3, and the lower right partition is a partition 4, and the partition where each obstacle is located is recorded;

and 5: as shown in fig. 6, a path point set is generated, the start point coordinates and the end point coordinates are added to the path point set, and a path segment is generated, wherein the path segment is a straight line connecting the start point and the end point because only two points, namely the start point and the end point, exist in the path point set at this time;

step 6: interference detection is carried out on a path section, the partition passed by the path section is 1 partition, 2 partition and 3 partition, so that 4 partitions of bounding boxes can be eliminated, interference detection is carried out only on the rest bounding boxes, the calculation amount of the interference detection can be reduced, as can be seen from fig. 6, the path section has interference with obstacles obsacle-1 and obsacle-2, the coordinates of interference points of the path section and the bounding boxes are calculated at the moment, the coordinates of the interference points are respectively calculated from the last point except the end point in a path point set, namely the Euclidean distance from the starting point, and the interference point corresponding to the minimum distance is selected as P;

and 7: the interference point P corresponds to the bounding box and has four vertexes ABCD, the path section divides the bounding box into two parts, namely AB and CD, and the sum of Euclidean distances from two points of the two parts to the path section is calculated respectively. Calculating to obtain the minimum sum of Euclidean distances from the AB two points to the path section, and selecting the AB two points as new candidate path points;

according to the distance from the candidate path point A, B to the starting point, the following operations are performed in sequence from small to large:

for point a: and the last point except the end point in the path point set is taken as a starting point, the starting point is O, the starting point O is connected with the point A to form a path section, the path section OA has no interference, and the point A is added into the path point set.

Point B is measured: and (3) taking the last point except the end point in the path point set as a point A, forming a path section by the connection point A and the point B, carrying out interference detection, adding the point B into the path point set when the path section AB is not interfered. After the candidate path point is judged, the path point set is updated, and a new path segment is generated as shown in fig. 7;

and 8: path optimization is performed, and optimization is not needed at this time;

and step 9: judging whether the end path interferes with the bounding box, as can be seen from fig. 7, when the path segment B and the end point interfere with the obstacle-2, repeating

steps

6, 7 and 8,

as shown in fig. 8, the path segment is divided into two parts, i.e., EF and GH, and the sum of euclidean distances from two points of the two parts to the path segment is calculated. After calculation, the sum of the Euclidean distances from the EF two points to the path section is minimum, so that the EF two points are selected as new candidate path points.

According to the distance from the candidate path point E, F to the starting point, the following operations are performed in sequence from small to large:

for point E: and the last point except the end point in the path point set is B, the connection point B and the point E form a path section and carry out interference detection on the path section, the path section BE has no interference, and the point E is added into the path point set.

At point F: and (3) the last point except the end point in the path point set is a point E, the connection point E and the point F form a path section and carry out interference detection, the path section EF has no interference, and the point F is added into the path point set. And (4) path optimization is performed, the point F and O, A, B in the path point set are respectively connected into line segments, interference detection is carried out, and connection abandoning is carried out when interference exists. O, A, B in the end point and path point sets are connected into line segments respectively, interference detection is carried out, and the connection is abandoned due to interference. Connecting the end point and the E point in the path point set into a line segment, carrying out interference detection without interference, deleting the path point F at the moment, and optimizing the path point set into O, A, B, E and the end point;

as shown in fig. 9, path optimization is performed, and the points F and O, A, B in the path point set are connected into path segments, respectively, and interference detection is performed, and the connection is abandoned by interference. O, A, B with concentrated end points and path points are respectively connected into path sections for interference detection, and the connection is abandoned by interference; connecting the end point and the E point in the path point set into a path section, carrying out interference detection without interference, deleting the path point F at the moment, and optimizing the path point set into O, A, B, E and the end point;

step 10: and smoothing the finally generated path. The rotation angle at point A, B, E is not favorable for tracking, and it is necessary to re-plan all path points using the curve to smooth the rotation angle, and after processing, the final path is generated as shown in fig. 10 and 11.

In the embodiment, the barrier and the robot are simplified by adopting a bounding box technology and equivalent expansion, the calculated amount is greatly reduced, path points are provided for a reconstructed path, and the path of the robot is reconstructed by adopting a vertex, so that the whole calculation process is simpler and more convenient, and the finally produced path is as short as possible, thereby greatly shortening the path traveling time of the robot, and simultaneously reducing the planning time.

While the invention has been described with reference to specific embodiments thereof, it will be understood by those skilled in the art that the invention is not limited thereto, and may be embodied in many different forms without departing from the spirit and scope of the invention as set forth in the following claims. Any modification which does not depart from the functional and structural principles of the present invention is intended to be included within the scope of the claims.

Claims

1. An algorithm for obstacle avoidance path planning, characterized by: the robot and obstacle detection method comprises a robot and an obstacle, wherein the space where the robot and the obstacle are located is a task space, and the algorithm comprises the following steps:

s2, simplified obstacle and robot: simplifying the robot into points, constructing bounding boxes for each obstacle,

and expanding the bounding box, the bounding box expanded by a width equal to the radius of the point;

2. An algorithm for obstacle avoidance path planning according to claim 1, wherein: the bounding box adopts an AABB bounding box or an OBB bounding box.

3. An algorithm for obstacle avoidance path planning according to claim 1, wherein: in S2, the task space is divided into a plurality of areas, the bounding boxes are numbered, and the areas where the bounding boxes are located are recorded.

4. An algorithm for obstacle avoidance path planning according to claim 3, wherein: when the action routes of the robots are on the same plane, the task space is divided into 4 partitions; when the action route of the robot is three-dimensional, the task space is divided into 8 partitions.

5. An algorithm for obstacle avoidance path planning according to claim 3, wherein: in S4, the partition in which the path segment is located is recorded, and the bounding box in the partition and the path segment are subjected to interference detection.

6. An algorithm for obstacle avoidance path planning according to claim 1, wherein: and in the step S5, when the path segment interferes with the bounding box, calculating the coordinates of interference points of the path segment and the bounding box, respectively calculating the euclidean distance between the coordinates of the interference points and the last point in the set of path points except the end point, and selecting the interference point corresponding to the minimum distance.

7. An algorithm for obstacle avoidance path planning according to claim 1, wherein: when the bounding boxes overlap, the overlapping bounding boxes are treated as one bounding box through the Boolean operation of addition.

8. An algorithm for obstacle avoidance path planning according to claim 1, wherein: and when the starting point or the end point is positioned in the range of the bounding box, taking the vertex of the obstacle or the vertex of the bounding box as the new starting point or the new end point.

9. An algorithm for obstacle avoidance path planning according to claim 8, wherein: when the starting point or the end point is located in the range of the bounding box, the straight line where the path section is located divides the bounding box into two parts, the sum of Euclidean distances from the top point of the bounding box included in the two parts to the straight line where the path section is located is calculated respectively, the top point of the bounding box included in the part with the smaller sum of the Euclidean distances and the top point of an obstacle on the edge of the bounding box are selected as candidate path points, the candidate path points and the starting point or the end point are connected into line segments respectively and are used for carrying out interference detection on the obstacle in the bounding box, the candidate path point P with the detection result of no interference is selected as a candidate escape point, the candidate escape point with the smaller Euclidean distance from the end point or the starting point is selected as an escape point, and the escape point is used as a new starting point or end point.