Fusion path planning method based on improved A and TEB algorithms

Abstract

The invention relates to the technical field of artificial intelligence, and discloses a fusion path planning method based on improved A and TEB algorithms, and the method is characterized in that the method comprises the following steps: S1, bidirectional stage searching; s2, dynamically weighting an evaluation function; s3, path smoothing processing; s4TEB algorithm fusion is carried out; and S5, outputting an optimal path. According to the fusion path planning method based on the improved A and TEB algorithms, search nodes and time are reduced through bidirectional stage search, the search speed and accuracy are improved by using a dynamic weighted evaluation function, path smoothing processing is performed by using a third-order Bezier curve, and global and local path planning fusion and dynamic obstacle avoidance are realized by combining the TEB algorithm. And an environment change adjustment strategy can be monitored in real time.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence technology, and in particular to a path planning method based on the fusion of improved A and TEB algorithms. Background Art

[0002] Robotics is a key research area in artificial intelligence, among which path planning is one of the essential core contents in robot research and development. It is the basis for robots to complete given tasks in a certain environment. The path planning ability of robots directly reflects their operational capabilities and risk management capabilities. Path planning refers to planning an optimal solution path from the starting point to the target point in a specified area, and ensuring that there is no collision with obstacles.

[0003] At present, the path planning method is very mature, but the problems of low search efficiency, many turning points and uneven paths still exist; and global or local path planning alone is not complete, and the two need to be integrated.

[0004] (1) Due to the complexity and changeability of the environment, path planning may not be suitable for all environments;

[0005] (2) There are often dynamic obstacles in the scene, which makes planning more difficult;

[0006] (3) Existing path planning algorithms search for too many nodes, resulting in long search times;

[0007] (4) When there are turning points in the path, the path is not smooth.

[0008] Therefore, a path planning method based on the fusion of improved A and TEB algorithms is proposed. Summary of the invention

[0009] 1. Technical issues to be resolved

[0010] In view of the shortcomings of the prior art, the present invention provides a path planning method based on the fusion of improved A and TEB algorithms, which has the advantages of adapting to complex environments, effectively dealing with dynamic obstacles, improving search efficiency, and generating smooth paths. It solves the problems of poor adaptability of path planning, great planning difficulty, long search time, and uneven path.

[0011] (II) Technical solution

[0012] In order to achieve the above-mentioned purposes of adapting to complex environments, effectively dealing with dynamic obstacles, improving search efficiency, generating smooth paths, etc., the present invention provides the following technical solutions: a path planning method based on improved A and TEB algorithm fusion, characterized in that it includes the following steps:

[0013] Bidirectional phase search: Adopting a bidirectional phase search strategy, searching from the starting point and the target point at the same time, and inserting a virtual navigation point in the middle to reduce the search nodes and search time;

[0014] Dynamic weighted evaluation function: Optimize the evaluation function of the traditional A algorithm and introduce a dynamic weighting factor into the heuristic function to speed up the search and improve the search accuracy;

[0015] Path smoothing: Use a third-order Bezier curve to smooth the planned path;

[0016] TEB algorithm fusion: The improved A algorithm is integrated with the TEB algorithm to achieve dynamic obstacle avoidance;

[0017] Output the optimal path.

[0018] As a preferred technical solution of the present invention, the bidirectional phase search strategy includes dynamically adjusting the search direction and step size during the search process to adapt to different environments.

[0019] As a preferred technical solution of the present invention, the dynamic weighting factor is dynamically adjusted according to the search progress, a larger weight is given in the early stage of the search to speed up the search, and a smaller weight is given when approaching the target point to prioritize the search for the best path.

[0020] As a preferred technical solution of the present invention, the third-order Bezier curve is used to perform smoothing at the turning points of the path to reduce the non-smoothness of the path.

[0021] As a preferred technical solution of the present invention, the fusion of the improved A algorithm and the TEB algorithm includes taking the global path planned by the A algorithm as the initial path of the TEB algorithm, and using the TEB algorithm to perform local trajectory optimization and dynamic obstacle avoidance.

[0022] As a preferred technical solution of the present invention, the fusion of the improved A algorithm and the TEB algorithm also includes real-time monitoring of environmental dynamics during the path planning process and adjusting the path planning strategy according to environmental changes.

[0023] As a preferred technical solution of the present invention, the path planning method is applicable to fields requiring path planning, such as robots and self-driving vehicles.

[0024] (III) Beneficial effects

[0025] Compared with the prior art, the present invention provides a path planning method based on the fusion of improved A and TEB algorithms, which has the following beneficial effects:

[0026] This path planning method is based on the fusion of improved A and TEB algorithms. It reduces search nodes and time through two-way stage search, uses dynamic weighted evaluation function to improve search speed and accuracy, uses third-order Bezier curve for path smoothing, and combines TEB algorithm to realize global and local path planning fusion and dynamic obstacle avoidance, and can monitor environmental changes in real time to adjust strategies. This method not only improves the efficiency and accuracy of path planning, but also enhances the smoothness of the path and the adaptability to the environment. It is widely used in robots, autonomous driving vehicles and other fields, and has significant technical advantages and application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 A schematic diagram of the method of the present invention. DETAILED DESCRIPTION

[0028] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0029] In the description of the present invention, it should be understood that the terms "center", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0030] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" 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 direct connection, or 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.

[0031] See also Figure 1 , a path planning method based on improved A and TEB algorithm fusion, characterized in that it includes the following steps:

[0032] S1 Bidirectional phase search: Adopting a bidirectional phase search strategy, searching from the starting point and the target point at the same time, and inserting a virtual navigation point in the middle to reduce the search nodes and search time;

[0033] S2 dynamic weighted evaluation function: optimizes the evaluation function of the traditional A algorithm and introduces a dynamic weighting factor into the heuristic function to speed up the search and improve the search accuracy;

[0034] S3 path smoothing: Use a third-order Bezier curve to smooth the planned path;

[0035] S4TEB algorithm fusion: The improved A algorithm is integrated with the TEB algorithm to achieve dynamic obstacle avoidance;

[0036] S4 outputs the optimal path.

[0037] The present invention adopts a two-way phase search strategy for path planning. The two-way phase search strategy is a search method that searches from a starting point and a target point at the same time and inserts a virtual navigation point in between. The method can significantly reduce the search nodes and search time, and improve the efficiency of path planning. During the search process, the present invention also dynamically adjusts the search direction and step size to adapt to different environments.

[0038] Secondly, the present invention optimizes the evaluation function of the traditional A algorithm. By introducing a dynamic weighting factor into the heuristic function, the present invention can speed up the search speed and improve the search accuracy. The dynamic weighting factor is dynamically adjusted according to the search progress, giving a larger weight in the early stage of the search to speed up the search speed, and giving a smaller weight when approaching the target point to prioritize the search for the best path.

[0039] Then, the present invention uses a third-order Bezier curve to smooth the planned path. At the turning point of the path, the third-order Bezier curve can effectively reduce the non-smoothness of the path, making the path more consistent with the motion state of the robot.

[0040] Next, the present invention integrates the improved A algorithm with the TEB algorithm. The global path planned by the A algorithm is used as the initial path of the TEB algorithm, and the TEB algorithm is used to optimize the local trajectory and dynamically avoid obstacles. The TEB algorithm can consider time information, make the local trajectory more consistent with the robot's kinematic constraints, and achieve dynamic obstacle avoidance.

[0041] In addition, the present invention monitors environmental dynamics in real time during the path planning process and adjusts the path planning strategy according to environmental changes. By sensing environmental changes in real time, the present invention can adjust the path planning solution in a timely manner to adapt to different application scenarios.

[0042] Finally, the method of the present invention is applicable to fields such as robots and self-driving vehicles that require path planning. By applying the method of the present invention, the efficiency and accuracy of path planning can be significantly improved, the search time and path roughness can be reduced, and dynamic obstacle avoidance can be achieved.

[0043] In this embodiment, the bidirectional phase search strategy includes dynamically adjusting the search direction and step size during the search process to adapt to different environments.

[0044] In this embodiment, the dynamic weighting factor is dynamically adjusted according to the search progress, a larger weight is given in the early stage of the search to speed up the search, and a smaller weight is given when approaching the target point to prioritize the search for the best path.

[0045] In this embodiment, the third-order Bezier curve is used to perform smoothing at the turning points of the path to reduce the non-smoothness of the path.

[0046] In this embodiment, the fusion of the improved A algorithm and the TEB algorithm includes taking the global path planned by the A algorithm as the initial path of the TEB algorithm, and using the TEB algorithm to perform local trajectory optimization and dynamic obstacle avoidance.

[0047] In this embodiment, the fusion of the improved A algorithm and the TEB algorithm also includes real-time monitoring of environmental dynamics during the path planning process and adjusting the path planning strategy according to environmental changes.

[0048] In this embodiment, the path planning method is applicable to fields that require path planning, such as robots and self-driving vehicles.

[0049] The beneficial effects of the above embodiment are:

[0050] Through bidirectional stage search, the search nodes and time are reduced, the dynamic weighted evaluation function is used to improve the search speed and accuracy, the third-order Bezier curve is used for path smoothing, and the TEB algorithm is combined to realize the fusion of global and local path planning and dynamic obstacle avoidance, and the environmental changes can be monitored in real time to adjust the strategy. This method not only improves the efficiency and accuracy of path planning, but also enhances the smoothness of the path and the adaptability to the environment. It is widely used in robots, autonomous driving vehicles and other fields, and has significant technical advantages and application prospects.

[0051] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method based on the fusion of improved A and TEB algorithms, characterized by: The following steps are involved: Bidirectional phase search: Adopting a bidirectional phase search strategy, searching from the starting point and the target point at the same time, and inserting a virtual navigation point in the middle to reduce the search nodes and search time; Dynamic weighted evaluation function: Optimize the evaluation function of the traditional A algorithm and introduce a dynamic weighting factor into the heuristic function to speed up the search and improve the search accuracy; Path smoothing: Use a third-order Bezier curve to smooth the planned path; TEB algorithm fusion: The improved A algorithm is integrated with the TEB algorithm to achieve dynamic obstacle avoidance; Output the optimal path.

2. The path planning method based on the fusion of improved A and TEB algorithms according to claim 1 is characterized in that: The bidirectional phase search strategy includes dynamically adjusting the search direction and step size during the search process to adapt to different environments.

3. The path planning method based on the fusion of improved A and TEB algorithms according to claim 1 is characterized in that: The dynamic weighting factor is dynamically adjusted according to the search progress. A larger weight is given in the early stage of the search to speed up the search, and a smaller weight is given when approaching the target point to prioritize the search for the best path.

4. The path planning method based on the fusion of improved A and TEB algorithms according to claim 1 is characterized in that: The third-order Bezier curve is used to perform smoothing at the turning points of the path to reduce the non-smoothness of the path.

5. The path planning method based on the fusion of improved A and TEB algorithms according to claim 1 is characterized in that: The fusion of the improved A algorithm and the TEB algorithm includes taking the global path planned by the A algorithm as the initial path of the TEB algorithm, and using the TEB algorithm to perform local trajectory optimization and dynamic obstacle avoidance.

6. The path planning method based on the fusion of improved A and TEB algorithms according to claim 1, characterized in that: The fusion of the improved A algorithm and the TEB algorithm also includes real-time monitoring of environmental dynamics during the path planning process and adjusting the path planning strategy according to environmental changes.

7. A path planning method based on improved A and TEB algorithm fusion according to any one of claims 1 to 6, characterized in that: The path planning method is applicable to fields such as robots and self-driving vehicles that require path planning.

Images