An Algorithm for Obstacle Avoidance Path Planning
Through the bounding box technology and path re-planning algorithm, the representation of obstacles and robots is simplified, the problem of large calculations of existing path planning algorithms is solved, and faster and more efficient path planning is achieved, suitable for real-time and resource-limited environments .
Patent Information
- Application Number
- CN202111549089.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-17
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2041-12-17
AI Technical Summary
In the case of obstacles, the existing path planning algorithm has large calculations or long search time, making it difficult to meet real-time requirements and limited hardware configuration scenarios.
The bounding box technology and equivalent expansion are used to simplify obstacles and robots, and through interference detection and path re-planning, a collision-free path is generated, and path smoothing is performed to reduce the calculation amount and path time.
This reduces the computational volume, simplifies the path planning process, shortens the robot's path time and planning time, and makes the algorithm more suitable for scenarios with high real-time requirements or poor hardware configuration computing performance, while improving the efficiency of path planning.
Smart Images

Figure CN114415665B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to an algorithm for obstacle avoidance path planning, and belongs to the technical field of path planning algorithms. Background Art
[0002] Path planning is an important part in the field of robotics. Its purpose is to find a collision-free path from the starting point to the end point that meets other constraints in the presence of obstacles.
[0003] The path planning algorithms widely used today include: D* algorithm, A* algorithm, rapid random tree (RRT) algorithm and its variants, probabilistic roadmap (PRM) algorithm and its variants, genetic algorithm, ant colony algorithm, artificial potential field method, etc. However, the above path planning algorithms have large computational complexity or long search time. Summary of the invention
[0004] The technical problem to be solved by the present invention is to provide an algorithm for obstacle avoidance path planning, which has less computational complexity.
[0005] To solve the above technical problems, the present invention adopts the following technical solutions:
[0006] An algorithm for obstacle avoidance path planning includes a robot and an obstacle, wherein the space where the robot and the obstacle are located is a task space, and the algorithm steps are as follows:
[0007] S1. Obtain and input information: obtain and input the starting point coordinates and the end point coordinates of the robot movement, as well as the coordinates of the vertices of each obstacle;
[0008] S2. Simplify obstacles and robots: simplify the robot into points, construct bounding boxes for each obstacle, and expand the bounding boxes. The expanded width of the bounding boxes is equal to the radius of the robot.
[0009] S3, generating a path point set and an initial path: generating a path point set, wherein the path point set is a set of path points ultimately used to form a path, adding the starting point coordinates and the end point coordinates to the path point set, and generating a path segment according to the path points in the path point set;
[0010] S4, interference detection: performing interference detection on the path segment and the bounding box;
[0011] S5, path replanning: when the path segment interferes with the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices contained in the two parts to the straight line where the path segment is located is calculated respectively, and the vertices contained in the part with the smaller sum of the Euclidean distances are selected as candidate path points, and the candidate path points are added in order from small to large before the coordinates of the end point according to the distance from the candidate path points to the starting point;
[0012] S6, connecting the newly added path point to other points in the path point set except the adjacent point, and judging whether the new path segment will interfere with the bounding box, if yes, abandon the connection, otherwise use the connection as a new path segment, and remove other path points between the two points, and update the path at the same time; directly connecting the end point to other points in the path point set except the adjacent point, and judging whether the new path segment will interfere with the bounding box, if yes, abandon the connection, otherwise use the connection as a new path segment, and remove other path points between the two points;
[0013] S8, determining whether all paths interfere with the bounding box, if yes, repeating step 5; otherwise, generating a collision-free path;
[0014] S9, path smoothing: smoothing the final generated collision-free path.
[0015] The beneficial effects of adopting the present invention are:
[0016] The present invention simplifies the obstacle and the robot by adopting bounding box technology and equivalent expansion, greatly reducing the amount of calculation and providing path points for reconstructing the path. The use of vertices to reconstruct the path of the robot can make the entire calculation process simpler and more convenient, and can also make the final produced path as short as possible, thereby greatly shortening the robot's travel time and reducing the planning time, making the present invention more suitable for use scenarios with high real-time requirements or poor hardware configuration computing performance. It can also be used in combination with other planning algorithms to achieve faster and better path planning effects.
[0017] Preferably, the bounding box adopts an AABB bounding box or an OBB bounding box.
[0018] Preferably, in S2, the task space is divided into a plurality of zones, the bounding boxes are numbered and the zones in which they are located are recorded.
[0019] Preferably, when the movement route of the robot is in the same plane, the task space is divided into 4 partitions; when the movement route of the robot is three-dimensional, the task space is divided into 8 partitions.
[0020] Preferably, in S4, the partition where the path segment is located is recorded, and interference detection is performed between the bounding box in the partition and the path segment.
[0021] Preferably, when the path segment and the bounding box interfere with each other in S5, the coordinates of the interference points between the path segment and the bounding box are calculated, and the Euclidean distances of the interference point coordinates from the last point in the path point set except the end point are calculated respectively, and the interference point corresponding to the smallest distance is selected.
[0022] Preferably, when the bounding boxes overlap, a Boolean operation of adding the overlapping bounding boxes is performed and the overlapping bounding boxes are regarded as one bounding box.
[0023] Preferably, when the starting 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 starting point or the end point.
[0024] Preferably, when the starting point or the end point is within the range of the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices of the bounding boxes contained in the two parts to the straight line where the path segment is located is calculated respectively, and the bounding box vertices contained in the part with the smaller sum of the Euclidean distances and the obstacle vertices on the edge of the bounding box are selected as candidate path points, and the candidate path points are connected to the starting point or the end point as line segments, and interference detection is performed on the obstacles in the bounding box, and the candidate path point P with a detection result of no interference is used as a candidate escape point, and among these candidate escape points, the one with a smaller Euclidean distance to the end point or the starting point is selected as the escape point, and the escape point is used as the new starting point or end point.
[0025] Other features and advantages of the present invention will be disclosed in detail in the following specific embodiments and drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] The present invention will be further described below in conjunction with the accompanying drawings:
[0027] Figure 1 A simplified flow chart of an embodiment of the present invention;
[0028] Figure 2 For the simulation of the embodiment of the present invention Figure 1 ;
[0029] Figure 3 For the simulation of the embodiment of the present invention Figure 2 ;
[0030] Figure 4 For the simulation of the embodiment of the present invention Figure 3 ;
[0031] Figure 5 For the simulation of the embodiment of the present invention Figure 4 ;
[0032] Figure 6 For the simulation of the embodiment of the present invention Figure 5 ;
[0033] Figure 7 For the simulation of the embodiment of the present invention Figure 6 ;
[0034] Figure 8 For the simulation of the embodiment of the present invention Figure 7 ;
[0035] Fig. 9 For the simulation of the embodiment of the present invention Figure 8 ;
[0036] Fig.10 For the simulation of the embodiment of the present invention Figure 9 ;
[0037] Fig.11 For the simulation of the embodiment of the present invention Figure 10 . DETAILED DESCRIPTION
[0038] The technical solutions of the embodiments of the present invention are explained and described below in conjunction with the drawings of the embodiments of the present invention, but the following embodiments are only preferred embodiments of the present invention, not all. Based on the embodiments in the implementation mode, other embodiments obtained by those skilled in the art without creative work are all within the protection scope of the present invention.
[0039] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "clockwise", "counterclockwise" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as limiting the present invention.
[0040] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, "plurality" means two or more, unless otherwise explicitly defined.
[0041] In the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", "connected", "fixed" and the like should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.
[0042] like Figure 1-11As shown, this embodiment shows an algorithm for obstacle avoidance path planning, including a robot and an obstacle, the space where the robot and the obstacle are located is a task space, and the algorithm steps are as follows:
[0043] S1. Obtain and input information: obtain and input the starting point coordinates and the end point coordinates of the robot movement, as well as the coordinates of the vertices of each obstacle;
[0044] S2. Simplify obstacles and robots: simplify the robot into points, construct bounding boxes for each obstacle, and expand the bounding boxes. The expanded width of the bounding boxes is equal to the radius of the robot.
[0045] S3, generating a path point set and an initial path: generating a path point set, wherein the path point set is a set of path points ultimately used to form a path, adding the starting point coordinates and the end point coordinates to the path point set, and generating a path segment according to the path points in the path point set;
[0046] S4, interference detection: performing interference detection on the path segment and the bounding box;
[0047] S5, path replanning: when the path segment interferes with the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices contained in the two parts to the straight line where the path segment is located is calculated respectively, and the vertices contained in the part with the smaller sum of the Euclidean distances are selected as candidate path points, and the candidate path points are added in order from small to large before the coordinates of the end point according to the distance from the candidate path points to the starting point;
[0048] S6, path optimization: connect the newly added path point to other points in the path point set except the adjacent point, and determine whether the new path segment will interfere with the bounding box. If yes, abandon the connection, otherwise use the connection as a new path segment, remove other path points between the two points, and update the path at the same time; directly connect the end point to other points in the path point set except the adjacent point, and determine whether the new path segment will interfere with the bounding box. If yes, abandon the connection, otherwise use the connection as a new path segment, and remove other path points between the two points;
[0049] S8, determining whether all paths interfere with the bounding box, if yes, repeating step 5; otherwise, generating a collision-free path;
[0050] S9, path smoothing: smoothing the final generated collision-free path.
[0051] The bounding box adopts an AABB bounding box or an OBB bounding box. In S2 of this embodiment, the task space is divided into multiple zones, the bounding boxes are numbered and the zones in which they are located are recorded. When the movement route of the robot is in the same plane, the task space is divided into 4 partitions; when the movement route of the robot is three-dimensional, the task space is divided into 8 partitions, and the partition where the path segment is located is recorded in S4, and interference detection is performed on the bounding boxes and path segments in the partitions.
[0052] In S5 of this embodiment, when the path segment interferes with the bounding box, the coordinates of the interference points between the path segment and the bounding box are calculated, and the Euclidean distances of the interference point coordinates from the last point in the path point set except the end point are calculated respectively, and the interference point corresponding to the smallest distance is selected.
[0053] When the bounding boxes overlap, the overlapping bounding boxes are added together and regarded as one bounding box. When the starting point or the end point is within the range of the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices of the bounding boxes contained in the two parts to the straight line where the path segment is located is calculated respectively. The bounding box vertices contained in the part with the smaller sum of the Euclidean distances and the obstacle vertices on the edge of the bounding box are selected as candidate path points. The candidate path points are connected to the starting point or the end point as line segments, and interference detection is performed on the obstacles in the bounding box. The candidate path point P with a detection result of no interference is used as a candidate escape point. Among these candidate escape points, the one with a smaller Euclidean distance to the end point or the starting point is selected as the escape point, and the escape point is used as the new starting point or end point.
[0054] like Figure 2-11 As shown, this embodiment takes a 9*11 two-dimensional task space as an example. There are 5 obstacles of different shapes in the task space. The starting coordinates of the robot are (1,1) and the end coordinates are (9,8). The path planning process of this embodiment is as follows:
[0055] Step 1: Figure 2 As shown, information is obtained and inputted. The starting coordinates of the robot are (1,1), the ending coordinates are (9,8), and the vertex coordinates of the five obstacles are also known.
[0056] Step 2: If Figure 3 As shown, the AABB bounding boxes of each obstacle component are shown. It can be understood that the AABB bounding box used in this embodiment simplifies the obstacle. For example, for obstacle-1, the maximum and minimum values Xmax, Xmi, Ymax, and Ymi of all vertices in the X and Y directions are calculated, and the X and Y values are combined in pairs as the vertices of the bounding box. Of course, in other embodiments, the OBB bounding box can also be used to simplify the obstacle;
[0057] Step 3: If Figure 4 As shown, the robot is simplified into a point, and the radius of the robot is equivalent to the expanded width of all bounding boxes. In this embodiment, the radius of the simplified robot is 0.2, and the width of the bounding box is equivalent to expanding by 0.2;
[0058] Step 4: Figure 5 As shown, this embodiment divides the task space into 4 equal partitions, where the upper right is partition 1, the upper left is partition 2, the lower left is partition 3, and the lower right is partition 4, and records the partition where each obstacle is located;
[0059] Step 5: Figure 6 As shown, a path point set is generated, the starting point coordinates and the end point coordinates are added to the path point set, and a path segment is generated. Since there are only two points, the starting point and the end point, in the path point set at this time, the path segment is a straight line connecting the starting point and the end point;
[0060] Step 6: Perform interference detection on the path segment. Since the path segment passes through partitions 1, 2, and 3, the bounding box of partition 4 can be excluded. Only the remaining bounding boxes are subjected to interference detection, which can reduce the amount of calculation for interference detection. Figure 6 It can be seen that the path segment interferes with obstacles obstacle-1 and obstacle-2. At this time, the coordinates of the interference points between the path segment and the bounding box are calculated, and the Euclidean distance of the interference point coordinates from the last point in the path point set except the end point, that is, from the starting point, is calculated respectively. The interference point corresponding to the smallest distance is selected as P;
[0061] Step 7: The bounding box corresponding to the interference point P has four vertices ABCD. The path segment divides it into two parts AB and CD, and calculates the sum of the Euclidean distances from the two points in the two parts to the path segment. After calculation, the sum of the Euclidean distances from the two points AB to the path segment is the smallest, so the two points AB are selected as new candidate path points;
[0062] According to the distance from candidate path points A and B to the starting point, perform the following operations in order from small to large:
[0063] For point A: the last point in the path point set except the end point is the starting point, the starting point is O, the starting point O and point A are connected to form a path segment and interference detection is performed on them. If there is no interference in the path segment OA, point A is added to the path point set.
[0064] For point B: The last point in the path point set except the end point is point A. Connect point A and point B to form a path segment and perform interference detection. If there is no interference between path segment AB, point B is added to the path point set. After the candidate path points are determined, the path point set is updated and a new path segment is generated. Figure 7 As shown;
[0065] Step 8: Perform path optimization. No optimization is required at this time.
[0066] Step 9: Determine whether the end path still interferes with the bounding box. Figure 7 It can be seen that at this time, the path segment between B and the end point interferes with obstacle-2. Repeat steps 6, 7, and 8.
[0067] like Figure 8 As shown in the figure, the path segment is divided into two parts, EF and GH, and the sum of the Euclidean distances from the two points in the two parts to the path segment is calculated. After calculation, the sum of the Euclidean distances from the two points EF to the path segment is the smallest, so the two points EF are selected as new candidate path points.
[0068] According to the distance from candidate path points E and F to the starting point, perform the following operations in order from small to large:
[0069] For point E: the last point in the path point set except the end point is B. Connect point B and point E to form a path segment and perform interference detection on it. If there is no interference in path segment BE, add point E to the path point set.
[0070] For point F: the last point in the path point set except the end point is point E. Connect point E and point F to form a path segment and perform interference detection. If there is no interference in path segment EF, add point F to the path point set. Perform path optimization, connect point F and O, A, and B in the path point set into line segments, perform interference detection, and abandon the connection if there is interference. Connect the end point and O, A, and B in the path point set into line segments, perform interference detection, and abandon the connection if there is interference. Connect the end point and point E in the path point set into a line segment, perform interference detection, and if there is no interference, delete path point F and optimize the path point set to O, A, B, E, and the end point;
[0071] like Fig. 9 As shown, perform path optimization, connect point F and point O, A, and B in the path point set into path segments, perform interference detection, and abandon the connection if there is interference. Connect the end point and point O, A, and B in the path point set into path segments, perform interference detection, and abandon the connection if there is interference; connect the end point and point E in the path point set into a path segment, perform interference detection, and there is no interference. At this time, delete the path point F, and the path point set is optimized to O, A, B, E, and the end point;
[0072] Step 10: Smooth the final generated path. At this time, the corners at points A, B, and E are not conducive to tracking motion. It is necessary to use curves to replan all path points to make the corners smooth. After processing, the final path is generated as follows: Fig.10 and 11 shown.
[0073] In this embodiment, the obstacle and the robot are simplified by adopting bounding box technology and equivalent expansion, which greatly reduces the amount of calculation and provides path points for reconstructing the path. The use of vertices to reconstruct the path of the robot can make the entire calculation process simpler and more convenient, and can also make the final produced path as short as possible, thereby greatly shortening the robot's travel time and reducing the planning time. This makes this embodiment more suitable for use scenarios with high real-time requirements or poor hardware configuration computing performance. It can also be used in combination with other planning algorithms to achieve faster and better path planning effects.
[0074] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Those skilled in the art should understand that the present invention includes but is not limited to the contents described in the drawings and the above specific embodiments. Any modification that does not deviate from the functional and structural principles of the present invention will be included in the scope of the claims.
Claims
1. An algorithm for obstacle avoidance path planning, characterized in that: Including robots and obstacles, the space where the robots and obstacles are located is the task space, and the algorithm steps are as follows: S1. Obtain and input information: obtain and input the starting point coordinates and the end point coordinates of the robot movement, as well as the coordinates of the vertices of each obstacle; S2. Simplify obstacles and robots: simplify the robot into points, construct a bounding box for each obstacle, the bounding box adopts an AABB bounding box or an OBB bounding box, and expands the bounding box. The expanded width of the bounding box is equal to the radius of the robot. S3, generating a path point set and an initial path: generating a path point set, wherein the path point set is a set of path points ultimately used to form a path, adding the starting point coordinates and the end point coordinates to the path point set, and generating a path segment 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 replanning: when the path segment interferes with the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices contained in the two parts to the straight line where the path segment is located is calculated respectively, and the vertices contained in the part with the smaller sum of the Euclidean distances are selected as candidate path points, and are added in order from small to large before the coordinates of the end point according to the distance from the candidate path point to the starting point; S6, connecting the newly added path point to other points in the path point set except the adjacent point, and judging whether the new path segment will interfere with the bounding box, if yes, abandon the connection, otherwise use the connection as a new path segment, and remove other path points between the two points, and update the path at the same time; directly connecting the end point to other points in the path point set except the adjacent point, and judging whether the new path segment will interfere with the bounding box, if yes, abandon the connection, otherwise use the connection as a new path segment, and remove other path points between the two points; S8, determining whether all paths interfere with the bounding box, if yes, repeating step 5; otherwise, generating a collision-free path; S9, path smoothing: smoothing the final generated collision-free path.
2. The algorithm for obstacle avoidance path planning according to claim 1, characterized in that: In S2, the task space is divided into a plurality of zones, the bounding boxes are numbered and the zones in which they are located are recorded.
3. The algorithm for obstacle avoidance path planning according to claim 2, characterized in that: When the robot's action route is on the same plane, the task space is divided into 4 partitions; when the robot's action route is three-dimensional, the task space is divided into 8 partitions.
4. The algorithm for obstacle avoidance path planning according to claim 2, characterized in that: In S4, the partition where the path segment is located is recorded, and interference detection is performed between the bounding box in the partition and the path segment.
5. The algorithm for obstacle avoidance path planning according to claim 1, characterized in that: In S5, when the path segment and the bounding box interfere with each other, the coordinates of the interference points between the path segment and the bounding box are calculated, and the Euclidean distances of the interference point coordinates from the last point in the path point set except the end point are calculated respectively, and the interference point corresponding to the minimum distance is selected.
6. The algorithm for obstacle avoidance path planning according to claim 1, characterized in that: When the bounding boxes overlap, a Boolean operation of adding the overlapping bounding boxes is performed and the overlapping bounding boxes are regarded as one bounding box.
7. The algorithm for obstacle avoidance path planning according to claim 1, characterized in that: When the starting 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 starting point or the end point.
8. The algorithm for obstacle avoidance path planning according to claim 7, characterized in that: When the starting point or the end point is within the range of the bounding box, the straight line where the path segment is located divides the bounding box into two parts, and the sum of the Euclidean distances from the vertices of the bounding boxes contained in the two parts to the straight line where the path segment is located is calculated respectively, and the bounding box vertices contained in the part with the smaller sum of the Euclidean distances and the obstacle vertices on the edge of the bounding box are selected as candidate path points, and the candidate path points are respectively connected to the starting point or the end point as line segments, and interference detection is performed on the obstacles in the bounding box, and the candidate path points P with a detection result of no interference are used as candidate escape points, and among these candidate escape points, the one with a smaller Euclidean distance to the end point or the starting point is selected as the escape point, and the escape point is used as the new starting point or end point.
