A hybrid path planning method and device based on an improved adaptive window algorithm

Abstract

This invention discloses a hybrid path planning method and apparatus based on an improved adaptive window algorithm, comprising: (1) obtaining an initial grid map of the environment through SLAM; (2) determining the robot's starting point S and target point G in the grid map; (3) performing global path planning using an improved A* algorithm; (4) finding the target point G using the improved A* algorithm to obtain a global path Path; (5) sequentially selecting the starting point, inflection point, and ending point from the global path Path as key points, and using them as a set of sub-target points for local path planning; (6) performing local path planning using an improved adaptive window algorithm; (7) when the robot finds the final target point G, completing the local path planning and completing the hybrid path planning from the starting point S to the target point G. Using this invention, the reliability and safety of the robot's obstacle avoidance and pathfinding process can be improved.

Description

Technical Field

[0001] This invention belongs to the field of path planning, and particularly relates to a hybrid path planning method and apparatus based on an improved adaptive window algorithm. Background Technology

[0002] Path planning is a key technology for autonomous mobile robots and a hot topic in mobile robot navigation research. Based on the nature of the environment and the target, path planning can be divided into global path planning (GPP) and local path planning (LPP). Common algorithms for global path planning include A* and Dijkstra's algorithm. For local path planning, common algorithms include artificial potential field methods, dynamic window algorithms, and adaptive window algorithms. While using global and local path planning algorithms can achieve obstacle avoidance and pathfinding, the safety level during robot movement remains insufficient. In some practical applications and environments, this level of safety is inadequate. Therefore, improving the safety and reliability of path planning algorithms in complex environments and finding the optimal path remains a significant technical challenge.

[0003] The traditional A* algorithm is mainly used in static situations where the surrounding environment is known. It is a heuristic traversal search algorithm based on Dijkstra's algorithm and Best First Search (BFS). When planning a path, it considers not only the distance between itself and the nearest node (Dijkstra's implementation) but also the distance between itself and the target point (BFS implementation). For example, Chinese patent document CN115046557A discloses an AGV path planning method combining B-spline curves and the A* algorithm. It obtains the starting point information, ending point information, and constraints of the path to be planned from the host computer, generates a starting direction control node based on the starting point information and the starting point control constraint distance, and generates an ending direction control node based on the ending point information and the ending point control constraint distance.

[0004] Compared with traditional traversal search algorithms, the traditional A* algorithm makes up for the greedy mechanism in the search process, thus completing the path planning task well. However, it is not suitable for dynamic environments, unknown environments, or partially unknown environments.

[0005] The Adaptive Window Approach (AWA) is an improvement on the Dynamic Window Approach (DWA). The AWA allows the planning window to adaptively adjust based on the path planning environment and set requirements, resulting in better path planning. The Dynamic Window Approach is a local path planning algorithm capable of avoiding obstacles in unknown environments. Its core idea is to determine a sampled velocity space (v, ω) that satisfies the robot's hardware constraints based on the robot's current position and velocity. Then, it calculates the robot's trajectory within this sampled velocity space over a certain time, evaluates the trajectory using an evaluation function, and finally selects the velocity corresponding to the best-evaluated trajectory as the robot's movement speed. This process is repeated until the robot reaches the target point. However, the Dynamic Window Approach only performs path planning when searching for sub-target points. Figure 6 The obstacle avoidance method shown, which extends the robot's radius by one on each side of the obstacle, is insufficient in terms of safety. The robot still faces some risk when navigating narrow passages. Furthermore, in practical applications, the robot's actual path deviates from the planned path, and the obstacle avoidance logic of the dynamic window algorithm is not sufficient to guarantee the safety of the robot's path planning. The adaptive window algorithm, while improving upon the dynamic window algorithm by allowing the planning window range to change adaptively, does not address the safety issues. In other words, the safety deficiencies inherent in the dynamic window algorithm persist in the adaptive window algorithm. Summary of the Invention

[0006] This invention provides a hybrid path planning method and apparatus based on an improved adaptive window algorithm, which can improve the reliability and safety of robot obstacle avoidance and path finding process.

[0007] A hybrid path planning method based on an improved adaptive window algorithm includes the following steps:

[0008] (1) Obtain an initial grid map of the environment using Simultaneous Localization and Mapping (SLAM) method;

[0009] (2) Determine the robot's starting point S and target point G in the grid map;

[0010] (3) Use the improved A* algorithm for global path planning; wherein, the improved A* algorithm introduces a risk function into the cost function, performs risk value prediction on grid nodes in the pathfinding process, and selects nodes with low cost value for expansion.

[0011] (4) The target point G is found by the improved A* algorithm, and the global path Path is obtained;

[0012] (5) Select the starting point, inflection point and ending point from the global path in sequence as key points, and use them as the sub-target point set when planning the local path; where the starting point in the first sub-target point set is S, and the ending point in the last sub-target point set is the target point G;

[0013] (6) Local path planning is performed using an improved adaptive window algorithm; wherein, the improved adaptive window algorithm combines the adaptive window algorithm with the artificial potential field method and designs a U-shaped trap escape logic.

[0014] (7) When the robot finds the final target point G, it completes the local path planning and the hybrid path planning from the starting point S to the target point G.

[0015] Furthermore, in step (3), the specific process of using the improved A* algorithm for global path planning is as follows:

[0016] (3-1) Create an open table and a close table to store nodes to be expanded and nodes that have been expanded, respectively;

[0017] (3-2) Calculate the cost function value of the starting point S and put S into the open table; the cost function is as follows:

[0018] F(n) = G(n) + H(n) + R(n)

[0019] In the formula, G(n) is the actual distance from the starting point S to the current point n, H(n) is the estimated distance from the current point n to the target point G, and R(n) is the risk value of the current point n.

[0020]

[0021] In the formula, α is a risk factor greater than zero, and r is the distance between the current point n and the nearest obstacle.

[0022] (3-3) Compare the cost function values ​​of each point in the open table. If the open table is empty, the algorithm fails. If the open table is not empty, select the node P with the smallest cost function value and move it from the open table to the close table.

[0023] (3-4) Determine whether point P is the target point G: if yes, proceed to step (3-7); if not, proceed to step (3-5).

[0024] (3-5) Expand and process the different types of neighboring nodes of P according to the logic of expanding neighboring nodes; the logic of expanding neighboring nodes is as follows:

[0025] There are three cases for neighboring nodes: if the neighboring node is an obstacle or is in the closed list, no action is taken; if the neighboring node has not been expanded, the cost function value of the neighboring node is calculated and added to the open list; if the neighboring node is already in the open list, the size of G(n) is compared to determine whether the parent node needs to be updated.

[0026] (3-6) Based on the situation in step (3-5), add or update the information of the nodes in the open table, and jump to step (3-3);

[0027] (3-7) Find the target point, trace back to the starting point based on the parent node, find the path, complete the global path planning, and end the loop.

[0028] In step (6), the specific process of using the improved adaptive window algorithm for local path planning is as follows:

[0029] (6-1) The robot obtains local environmental information through lidar, determines the lidar range d, and divides the lidar range into n equal parts;

[0030] (6-2) Adaptively adjust the planning window range based on the environmental information detected by the lidar; expand the planning window range when the feasible area within the planning window is large; shrink the planning window range when the feasible area is small; determine the window range r. win Angular resolution Δ Φ The robot is facing G. R Information such as...

[0031] (6-3) The current searched sub-target point is G. t The search is performed on points in the order of the sub-target point sets.

[0032] (6-4) Determine whether the final target point G has been found; if yes, end the loop and jump to step (10); if no, proceed to step (6-5).

[0033] (6-5) Determine if there is G within the planning window. t Sub-target point G after point t+x Or there is G t Point and G t+1 If point R is on the line connecting the points, proceed to step (6-6); otherwise, jump to (6-7).

[0034] (6-6) Change the currently searched sub-target point to G. t+x Point R or R, proceed to steps (6-7) to continue searching for the modified sub-target point;

[0035] (6-7) Introduce the repulsive potential field from the artificial potential field method and expand the obstacles within the planning window to determine the feasible area within the planning window; the specific process is as follows:

[0036] Based on the robot's radius r rob The edges of the obstacle are extended, and the obstacle and its extended portion are considered infeasible regions. Simultaneously, a repulsive potential field from the artificial potential field method is introduced, and for the repulsive value F... req Directions exceeding preset values ​​are also considered infeasible areas; except for infeasible areas, other parts within the planning window are feasible areas.

[0037] (6-8) Determine if there is a feasible area within the planning window: if there is, proceed to (6-9); if not, jump to (6-11).

[0038] (6-9) After determining the feasible region, combine the nearest sub-target point G within the feasible region. t Given the direction of the robot and its current orientation, determine the robot's direction of movement for the next moment, and jump to (6-1) to loop;

[0039] (6-10) End the local path planning loop and jump to step (7);

[0040] (6-11) Determine if the robot is trapped in a U-shaped trap and execute the U-shaped trap escape logic. The specific U-shaped trap escape logic is as follows:

[0041] Establish a Cartesian coordinate system with the robot's current position as the origin and the current orientation as the positive direction. Based on the currently searched sub-target point G... t Local path planning is performed based on the robot's positional relationship;

[0042] When the sub-target point G t Located in the first quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the right at a certain angle until there is a feasible area within the planning window.

[0043] When the sub-target point G t Located in the second quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the left at a certain angle until there is a feasible area within the planning window.

[0044] When the sub-target point G t Located in the third and fourth quadrants, the robot did not fall into the U-shaped trap. Following the logic of the improved adaptive window algorithm, the robot autonomously searched for the sub-target point G. t .

[0045] A hybrid path planning device based on an improved adaptive window algorithm includes a memory and one or more processors. The memory stores executable code, and when the one or more processors execute the executable code, they implement the hybrid path planning method described above.

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] 1. This invention studies the robot path planning problem in complex environments. By considering different environmental and robot information, the path planning in complex dynamic environments is divided into two parts: global (static) path planning and local (dynamic) path planning. The two parts are independent of each other, yet they complement each other. The algorithmic logic of the two parts together constitutes this invention, enabling it to achieve highly reliable and safe path finding and obstacle avoidance even in complex dynamic environments.

[0048] 2. This invention uses an improved A* algorithm in global path planning. Compared with the traditional A* algorithm, the cost function is modified by introducing a risk function to estimate the risk value of grid points during the pathfinding process, thereby selecting grid points with lower cost and lower risk values ​​for expansion. This algorithm can be directly used on the initial grid map of the environment obtained through Simultaneous Localization and Mapping (SLAM), reducing the complexity of map processing and improving the security of the final pathfinding result.

[0049] 3. In local path planning, this invention employs an improved adaptive window algorithm, combining it with an artificial potential field method to handle obstacles that suddenly appear in the local environment. During pathfinding, the planning window range adjusts autonomously based on the size of the feasible region, improving path planning efficiency. Simultaneously, the repulsive potential field introduced in the artificial potential field method can determine the repulsive force value within the planning window range, further filtering feasible regions. Furthermore, the repulsive force value can be autonomously set according to the actual application environment and objective needs, selecting regions with smaller repulsive force values ​​and greater distance from obstacles as feasible regions, thus improving safety during pathfinding.

[0050] 4. This invention incorporates consideration of U-shaped traps. In complex environments, suddenly appearing obstacles may form U-shaped traps with existing obstacles. When the robot enters a U-shaped trap, path planning will fail because there is no feasible area within the planning window. Therefore, this invention designs U-shaped trap escape logic, prompting the robot to perform corresponding actions to escape the U-shaped trap in three different scenarios where there is no feasible area within the planning window. This ensures that the robot can continue to find its way normally and improves the reliability of pathfinding. Attached Figure Description

[0051] Figure 1This is a flowchart of a hybrid path planning method based on an improved adaptive window algorithm according to the present invention.

[0052] Figure 2 This is a flowchart illustrating global path planning using the improved A* algorithm in this invention.

[0053] Figure 3 The flowchart shows the local path planning using the improved adaptive window algorithm in this invention.

[0054] Figure 4 This is a flowchart of the U-shaped trap escape logic used in this invention;

[0055] Figure 5 This is a schematic diagram comparing the path planning results based on the improved A* algorithm in this invention with the path planning results based on the traditional A* algorithm.

[0056] Figure 6 This is a schematic diagram of the adaptive planning window and obstacle edge expansion in this invention. Detailed Implementation

[0057] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be noted that the embodiments described below are intended to facilitate the understanding of the present invention and do not constitute any limitation thereof.

[0058] like Figure 1 As shown, a hybrid path planning method based on an improved adaptive window algorithm includes the following steps:

[0059] S1, obtain the initial grid map by processing image information through SLAM; input basic robot information, such as robot radius r. rob Maximum speed v max Maximum angular velocity w max wait.

[0060] S2, determine the starting point S and the target point G in the raster map.

[0061] S3(GPP) uses the safe A* algorithm (an improved version of the A* algorithm) for global path planning.

[0062] like Figure 2 As shown, the specific process of the safe A* algorithm path planning is as follows:

[0063] GPP(1) creates open and close tables to store nodes to be expanded and nodes that have already been expanded, respectively.

[0064] GPP(2) calculates the cost function value of the starting point S and puts S into the open table. The cost function is as follows:

[0065] F(n) = G(n) + H(n) + R(n)

[0066] In the formula, G(n) is the actual distance from the starting point S to the current point n, H(n) is the estimated distance from the current point n to the target point G, and R(n) is the risk value of the current point n.

[0067]

[0068] In the formula, α is a risk factor greater than zero, and r is the distance between the current point n and the nearest obstacle.

[0069] GPP(3) compares the cost function values ​​of each point in the open table. If the open table is empty, the algorithm fails; if the open table is not empty, the node P with the smallest cost function value is selected and moved from the open table to the close table.

[0070] GPP(4) determines whether point P is the target point G: if yes, proceed to step GPP(7); if not, proceed to step GPP(5).

[0071] GPP(5) expands and processes the neighboring nodes of different types of P according to the logic of expanding neighboring nodes.

[0072] GPP(6) adds or updates information to the nodes in the open table based on the situation in step GPP(5), and then jumps to step GPP(3).

[0073] GPP(7) finds the target point, reverses back to the starting point based on the parent node, finds the path, completes global path planning, and ends the loop.

[0074] S4. The target point G is found using the secure A* algorithm, resulting in the global path Path. The global path obtained by the secure A* algorithm is compared with the path obtained by the traditional A* algorithm as follows: Figure 5 As shown in the figure, the left figure shows the path of the traditional A* algorithm, and the right figure shows the path of the safe A* algorithm. It can be seen that after the safe A* algorithm incorporates the risk function into the cost function, it always maintains a certain distance from obstacles, thus improving safety and reliability in path planning compared to the traditional A* algorithm.

[0075] S5: Select the starting point, inflection point and ending point from the global path in sequence as key points, and use them as the set of sub-target points for local path planning (the first set of points is the starting point S, and the last set of points is the target point G).

[0076] S6(LPP) uses an improved adaptive window algorithm for local path planning.

[0077] like Figure 3 As shown, the improved adaptive window algorithm path planning process is as follows:

[0078] LPP(1), the robot obtains local environmental information through LiDAR, determines the LiDAR range d, and divides the LiDAR range into n equal parts, such as Figure 6 As shown.

[0079] LPP(2) adaptively adjusts the planning window range based on the environmental information detected by the lidar. When the feasible area within the planning window is large, the planning window range is expanded; when the feasible area is small, the planning window range is reduced. The window range r is determined. win Angular resolution Δ Φ The robot is facing θ R Information such as...

[0080] LPP(3), the current searched sub-target point is G. t Points; search according to the order of the sub-target point set.

[0081] LPP(4) determines whether the final target point G has been found; if yes, the loop ends and jumps to step LPP(10); if no, step LPP(5) is performed.

[0082] LPP(5) determines whether there is G within the planning window. t Sub-target point G after point t+x Or there may be G t Point and G t+1 Point R on the line connecting the points: If it exists, proceed to step LPP(6); otherwise, jump to LPP(7).

[0083] LPP(6) changes the currently searched sub-target points to G as needed. t+x Point or R, perform LPP(7), and continue searching for the modified sub-target point.

[0084] LPP(7) determines the feasible region within the planning window range based on the improved adaptive window algorithm. This is based on the robot's radius r. rob The edges of obstacles are extended, and the obstacle and its extended portion are considered infeasible regions. Specifically, at the point of tangency between the robot and the obstacle, a line segment perpendicular to the tangent, with a length equal to the robot's radius, is drawn, forming a right-angled triangle. The same operation is performed at the other tangent point. Simultaneously, a repulsive potential field from the artificial potential field method is introduced, and for the repulsive force value F... req The larger directions are also considered infeasible areas. Apart from the infeasible areas, the rest of the planning window is considered feasible.

[0085] LPP(8) determines whether there is a feasible area within the planning window: if there is, proceed with LPP(9); if not, determine whether to enter a U-shaped trap and jump to LPP(11).

[0086] LPP(9), after determining the feasible region, combines the closest sub-target point G within the feasible region. t Given the direction of the robot and its current orientation, determine the robot's direction of movement for the next moment, and jump to LPP(1) for looping.

[0087] LPP(10) ends the local path planning loop and jumps to step S7.

[0088] LPP(11) determines whether the robot is trapped in a U-trap and executes the U-trap escape logic.

[0089] like Figure 4 As shown, the escape logic for the U-shaped trap is as follows:

[0090] Establish a Cartesian coordinate system with the robot's current position as the origin and the current orientation as the positive direction. Based on the currently searched sub-target point G... t Local path planning is performed based on the robot's positional relationship;

[0091] When the sub-target point G t Located in the first quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the right at a certain angle until there is a feasible area within the planning window.

[0092] When the sub-target point G t Located in the second quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the left at a certain angle until there is a feasible area within the planning window.

[0093] When the sub-target point G t Located in the third and fourth quadrants, the robot did not fall into the U-shaped trap. Following the logic of the improved adaptive window algorithm, the robot autonomously searched for the sub-target point G. t .

[0094] S7. When the robot finds the final target point G, it completes the local path planning and the hybrid path planning from the starting point S to the target point G.

[0095] In summary, the robot completed the entire hybrid path planning process from global to local perspectives. The two parts are independent yet mutually supportive, jointly enabling the robot to avoid obstacles, find its way, and track in complex dynamic environments, thus improving the safety and reliability of the robot's movement.

[0096] The embodiments described above provide a detailed explanation of the technical solutions and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, additions, and equivalent substitutions made within the scope of the principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A hybrid path planning method based on an improved adaptive window algorithm, characterized in that, Includes the following steps: (1) Obtain an initial grid map of the environment using the Simultaneous Localization and Mapping (SLAM) method; (2) Determine the robot's starting point S and target point G in the grid map; (3) Use the improved A* algorithm for global path planning; wherein the improved A* algorithm introduces a risk function into the cost function, performs risk value prediction on grid nodes in the pathfinding process, and selects nodes with low cost value for expansion; (4) The target point G is found using the improved A* algorithm, and the global path Path is obtained; (5) Select the starting point, inflection point and ending point from the global path in sequence as key points, and use them as the sub-target point set when planning the local path; where the starting point in the first sub-target point set is S, and the ending point in the last sub-target point set is the target point G; (6) Local path planning is performed using an improved adaptive window algorithm; wherein, the improved adaptive window algorithm combines the adaptive window algorithm with the artificial potential field method, and a U-shaped trap escape logic is designed; the specific process is as follows: (6-1) The robot obtains local environmental information through lidar, determines the lidar range d, and divides the lidar range into n equal parts; (6-2) Adaptively adjust the planning window range based on the environmental information detected by the lidar; (6-3) The current searched sub-target point is The search is performed on points in the order of the sub-target point sets; (6-4) Determine whether the final target point G has been found; if yes, end the loop and jump to step (10); if no, proceed to step (6-5). (6-5) Determine whether there is a plan within the planning window. Sub-target points after the point Or there is Point and If point R is on the line connecting the points, proceed to step (6-6); otherwise, jump to step (6-7). (6-6) Change the current search sub-target point to Point R or point R, proceed to steps (6-7), and continue searching for the modified sub-target point; (6-7) Introduce the repulsive potential field in the artificial potential field method and expand the obstacles within the planning window to determine the feasible area within the planning window; (6-8) Determine if there is a feasible area within the planning window: if there is, proceed to (6-9); if not, jump to (6-11). (6-9) After determining the feasible area, combine the nearest sub-target point within the feasible area. Given the direction of the robot and its current orientation, determine the robot's direction of movement for the next moment, and jump to (6-1) to loop; (6-10) End the local path planning loop and jump to step (7); (6-11) Determine if the robot is trapped in a U-shaped trap and execute the U-shaped trap escape logic; the specific U-shaped trap escape logic is as follows: Establish a Cartesian coordinate system with the robot's current position as the origin and the current orientation as the positive direction, based on the currently searched sub-target points. Local path planning is performed based on the robot's positional relationship; When the sub-target point Located in the first quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the right at a certain angle until there is a feasible area within the planning window. When the sub-target point Located in the second quadrant, the robot is trapped in a U-shaped trap. The robot is instructed to turn to the left at a certain angle until there is a feasible area within the planning window. When the sub-target point Located in the third and fourth quadrants, the robot did not fall into the U-shaped trap. Following the logic of the improved adaptive window algorithm, the robot autonomously searched for sub-target points. ; (7) When the robot finds the final target point G, it completes the local path planning and the hybrid path planning from the starting point S to the target point G.

2. The hybrid path planning method based on the improved adaptive window algorithm according to claim 1, characterized in that, In step (3), the specific process of using the improved A* algorithm for global path planning is as follows: (3-1) Create an open table and a close table to store nodes to be expanded and nodes that have already been expanded, respectively; (3-2) Calculate the cost function value of the starting point S and put S into the open table; (3-3) Compare the cost function values ​​of each point in the open table. If the open table is empty, the algorithm fails. If the open table is not empty, select the node P with the smallest cost function value and move it from the open table to the close table. (3-4) Determine whether point P is the target point G: if yes, proceed to step (3-7); if not, proceed to step (3-5). (3-5) Expand and process the neighboring nodes of different types of P according to the logic of expanding neighboring nodes; (3-6) Based on the situation in step (3-5), add or update the information of the nodes in the open table, and jump to step (3-3). (3-7) Find the target point, trace back to the starting point based on the parent node, find the path, complete the global path planning, and end the loop.

3. The hybrid path planning method based on the improved adaptive window algorithm according to claim 2, characterized in that, In step (3-2), the cost function is constructed to calculate the cost function value of the starting point S, where the cost function is as follows: In the formula, It is the actual distance from the starting point S to the current point n. It is the estimated distance from the current point n to the target point G. It is the risk value of the current point n; In the formula, It is a risk factor greater than zero, and r is the distance between the current point n and the nearest obstacle.

4. The hybrid path planning method based on the improved adaptive window algorithm according to claim 1, characterized in that, In step (6-2), when the feasible area within the planning window is large, the planning window range is expanded; when the feasible area is small, the planning window range is reduced; thus, the window range is determined. angular resolution Robot facing .

5. The hybrid path planning method based on the improved adaptive window algorithm according to claim 1, characterized in that, Steps (6-7) are the specific process as follows: Based on the robot's radius The edges of the obstacle are extended, and the obstacle and its extended portion are considered infeasible regions. Simultaneously, a repulsive potential field from the artificial potential field method is introduced, and the repulsive value is... Directions exceeding preset values ​​are also considered infeasible areas; except for infeasible areas, other parts within the planning window are feasible areas.

6. A hybrid path planning device based on an improved adaptive window algorithm, characterized in that, The system includes a memory and one or more processors, wherein the memory stores executable code, and the one or more processors execute the executable code to implement the hybrid path planning method according to any one of claims 1-5.

Images