A comprehensive scene map path planning method based on improved A* algorithm
Through the improved A* algorithm's bidirectional search strategy, obstacle line density weighting, and path quadratic optimization technology, the problems of low computational efficiency, dense path turning points, and intersection offset in comprehensive scene maps are solved, generating an efficient and smooth path planning solution.
Patent Information
- Application Number
- CN202511047505.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-29
- Publication Date
- 2025-09-30
- Estimated Expiration
- 2045-07-29
AI Technical Summary
The existing A* algorithm has problems in comprehensive scene maps, such as low computational efficiency, dense path turning points that do not meet actual motion control requirements, and the offset of the intersection point of the two-way search path resulting in an excessively long path.
An improved A* algorithm is adopted to generate a smooth trajectory that meets kinematic constraints through an improved bidirectional search strategy, a heuristic function of obstacle line density weight, variable neighborhood node expansion and path secondary optimization technology, including an improved formula of bidirectional search strategy, obstacle line density calculation, variable neighborhood search and path smoothing processing.
It significantly reduces the number of path search nodes, the average number of turns and planning time, and improves planning efficiency and path executability in complex environments.
Smart Images
Figure CN120558239B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of path planning technology, and in particular to a comprehensive scene map path planning method based on an improved A* algorithm. Background Art
[0002] With the rapid development of intelligent warehousing and logistics, service robotics, and other fields, path planning, as a core technology for autonomous mobile robots to achieve environmental perception and decision-making, has become increasingly important. The performance of its algorithms is directly related to the efficiency of intelligent agents in complex scenarios. Among traditional global path planning algorithms, the Dijkstra algorithm, while ensuring optimality, suffers from computational redundancy. The RRT and PRM series algorithms exhibit strong randomness, resulting in insufficient path stability. Swarm intelligence algorithms such as particle swarms and ant colonies converge slowly, making them unsuitable for engineering applications. The A* algorithm, however, introduces a heuristic function to guide search direction, achieving a good balance between time complexity and path quality, making it a mainstream solution for path planning in static environments.
[0003] Existing research on the A* algorithm focuses on optimizing the heuristic function and innovating the search strategy. Researchers have introduced the cosine of the vector angle to improve the traditional A* algorithm, enhancing the search direction. They have also expanded and optimized the search neighborhood to improve search efficiency. They have added an unknown path cost estimation function to the heuristic function to measure the estimated cost, bringing it closer to the actual cost. They have also considered not only the distance factor in the heuristic function but also the impact of obstacles around the current node. To shorten search time and reduce the number of redundant nodes in path planning, they have proposed an improved A* algorithm that replaces the eight-neighborhood with an obstacle-free rectangle when expanding nodes. Other researchers have combined the A* algorithm with intelligent algorithms. For example, researchers first use the Sparrow algorithm to obtain several key grid points in a grid map and then use the A* algorithm for quadratic planning. They also use the A* algorithm to calculate the initial path, using key nodes on the path as initial particles in a PSO. They then use a particle swarm optimization algorithm combined with random inertia weights and a randomized adversarial learning strategy to obtain the global optimal path. The resulting path is high-quality but computationally complex.
[0004] These methods perform well in specific scenarios or scenarios with a single type of obstacle, but they still face three major challenges in comprehensive scene maps (such as home environments that also contain semi-enclosed obstacles, narrow passages, and irregular boundaries): (1) Traditional fixed-step search generates a large number of invalid nodes, resulting in low computational efficiency; (2) The path turning points are dense and the corners are steep, which does not meet the actual motion control requirements; (3) The offset of the intersection point of the bidirectional search path causes the path to be too long. Summary of the Invention
[0005] Purpose of the invention: The technical problem to be solved by the present invention is to address the deficiencies of the existing technology and provide a comprehensive scene map path planning method based on an improved A* algorithm, comprising the following steps:
[0006] Step 1: Perform basic environment modeling for the comprehensive scene map: Divide the two-dimensional plane scene into two or more regular quadrilateral grids, and assign values to each grid in a binary manner to represent the grid occupancy status. The value of the grid occupied by the obstacle is set to 1 and filled with black, and the value of the remaining grid is set to 0 and filled with white. Construct a protection zone consisting of a single layer of grids around the obstacle and fill it with gray. Set the radius of the mobile robot's circumscribed circle to R, and the safe distance between the mobile robot and the obstacle to r, with r as the side length of the grid.
[0007] Step 2: Use the improved A* algorithm to plan the path, including the following steps:
[0008] Step 2-1, establishing an improved bidirectional search strategy;
[0009] Step 2-2, establish a heuristic function with obstacle line density weight;
[0010] Step 2-3: Use the node expansion method of changing the neighborhood to complete the node expansion;
[0011] Steps 2-4: perform secondary path optimization.
[0012] Step 2-1 includes: the formula of the improved bidirectional search strategy is:
[0013] (1) ,
[0014] in, Indicates the starting point The current node of the forward search path The distance between the evaluation function value, Indicates the target point To the current node of the backward search path The distance evaluation function value between them; d represents the current node of the forward search path The current node of the backward search path The Manhattan distance between Represents the current node of the forward search path With the target point The Manhattan distance between Represents the current node of the backward search path and starting point The Manhattan distance between them.
[0015] Step 2-1 also includes: simplifying formula (1) to obtain:
[0016] (2).
[0017] Step 2-2 includes:
[0018] The current node of the forward path and the current node of the backward path The obstacle line density between is added as a regulating factor to the heuristic function. First, the Bresenham algorithm is used to obtain and Connect the incremental grid list in the line, then traverse the incremental grid list to calculate the proportion of obstacle grids, so as to obtain the obstacle density of the path;
[0019] Assume that the slope of the line connecting the forward path and the backward path is k:
[0020] (3),
[0021] in The current node of the backward path The vertical coordinate of is the current node of the forward path The vertical coordinate of The current node of the backward path The horizontal axis of Current node on the forward path The horizontal axis of Indicates the increment in the horizontal direction; Indicates the increment in the vertical direction;
[0022] On the slope In the case of Larger, adjust the horizontal coordinate position of the corresponding node in steps of 1 in the horizontal direction, set and On the connected straight line The coordinates of the nodes are , the horizontal coordinate value on the straight line is The vertical coordinate value corresponding to the point is:
[0023] (4),
[0024] in, for and The horizontal coordinate on the straight line connected to The vertical coordinate value corresponding to the point; b is and The intercept of the straight line connecting .
[0025] Step 2-2 also includes:
[0026] calculate 、 as well as 、 The difference :
[0027] (5),
[0028] in, The coordinates are The grid to and The difference in ordinates of the connected straight lines, The coordinates are The grid to and The difference in ordinates of the connected straight lines, for and On the connected straight line The vertical coordinate value of the node; calculate the Decision parameters for the first step :
[0029] (6),
[0030] when hour , the point below the straight line is closer to the straight line. ;when hour , the point above the straight line is closer to the straight line. ,Will Substitute into formula (6) to find the decision parameter of step i+1 :
[0031] (7),
[0032] Use starting point The first node coordinates next to it are calculated to obtain the decision parameters of the first incremental grid :
[0033] (8),
[0034] get After that, the subsequent incremental grid decision parameters are deduced by formula (7), and the incremental grid decision parameters are used to determine the distribution of the incremental grid on the vertical coordinate until the current node of the backward path is encountered. , get the incremental grid list between the forward path and the backward path .
[0035] Step 2-2 also includes:
[0036] Iterate over the incremental grid list Nodes in the grid, calculate the linear density of obstacles :
[0037] (9) ,
[0038] Among them, num(obstacle) is the number of obstacle nodes; num(node) is the number of all nodes.
[0039] Finally, combined with formula (2), we can get the heuristic function of the forward path: and the heuristic function of the backward path :
[0040] (10).
[0041] Step 2-3 includes: taking the current node of the opposite path as the target point , establish a local coordinate system with the current node o as the origin, define the horizontal rightward direction as the positive direction of the x-axis and the vertical upward direction as the positive direction of the y-axis, and divide the position of the target point relative to the current node into eight cases, namely four different quadrant areas and four directions of the coordinate axis:
[0042] The first case is: target point Located in the first quadrant area;
[0043] The second case is: target point Located in the second quadrant area;
[0044] The third case is: target point Located in the third quadrant area;
[0045] The fourth case is: target point Located in the fourth quadrant area;
[0046] Fifth case: target point Located in the positive direction of the x-axis;
[0047] The sixth case: target point Located in the positive direction of the y-axis;
[0048] The seventh case: target point Located in the negative direction of the x-axis;
[0049] Case 8: Target point Located in the negative direction of the y-axis;
[0050] If the target point Relative to the current node The position is the first, second, third, and fourth cases: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to The other three nodes to be expanded in the same direction and the current node Expand 4 nodes in the up, down, left, and right directions, and expand 8 nodes in total; if the node If it is an obstacle node, only the current node Expand 4 nodes in the up, down, left, and right directions, for a total of 5 nodes;
[0051] If the position of the target point relative to the current node is the fifth, sixth, seventh, or eighth case: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to One node in the same direction around the current node In the up, down, left, and right directions, except for the nodes 3 nodes other than the one above are expanded, for a total of 5 nodes; if the node If it is an obstacle node, only the current node In the up, down, left, and right directions, except for the nodes Expand the other 3 nodes, and expand a total of 4 nodes.
[0052] Steps 2-4 include:
[0053] Step 2-4-1, broken line optimization;
[0054] Prune the path: First, the first node Add it to the optimized path list as the starting point, and judge the subsequent nodes in the original path in turn. Whether the connection line crosses the obstacle, after traversing the second node , the third node , the fourth node Afterwards, it was discovered With the fifth node If there is an obstacle between Previous node Add to the optimization path list and update As the new starting point, continue to traverse the nodes in the original path. If you find With the ninth node If there are obstacles between Previous node Add the optimized path list and update the starting point. Repeat steps 2-4-1 until the end of the path is reached. Finally, add the end point to the optimized path list.
[0055] Step 2-4-2, use piecewise cubic Hermite interpolation to smooth the path: set the path after interpolation to have discrete nodes ,in for The discrete nodes The horizontal coordinate of each discrete node; For the The horizontal coordinates of discrete nodes The corresponding vertical coordinate is then defined as follows for the data spacing and normalization parameters between adjacent nodes:
[0056] (11),
[0057] in: is the length of the i-th interpolation interval; z is the horizontal coordinate of the interpolation to be calculated; t is the normalization parameter, and the interval Mapping to intervals ;
[0058] Next, calculate the average gradient of each interval:
[0059] (12),
[0060] in ;
[0061] In order to avoid oscillation during interpolation and maintain monotonicity, the node derivatives are also determined at each interpolation node. ,if and Different signs, then the derivative Set to 0; if and If they have the same sign, they are calculated as follows:
[0062] (13),
[0063] So far, in each interval Construct the third piecewise cubic Hermite interpolation polynomial:
[0064] (14),
[0065] in represents the cubic interpolation polynomial on the jth segment; 、 Represent the gradient value at the jth interpolation node and the The gradient value at the interpolation node.
[0066] The present invention also provides an electronic device, comprising a processor and a memory, wherein the memory stores program code, and when the program code is executed by the processor, the processor executes the steps of the method.
[0067] The present invention also provides a storage medium storing a computer program or instruction, which executes the steps of the method when the computer program or instruction is run on a computer.
[0068] Beneficial effects: The present invention designs a multi-stage optimization scheme by analyzing the defects of the existing A* algorithm in complex maps, such as redundant node expansion, two-way search oscillation, and sharp path turns: first, a variable step-size search mechanism is introduced to dynamically adjust the search step size according to the azimuth angle between the node and the target to reduce invalid exploration; secondly, an adaptive two-way search strategy is constructed to guide the path to efficiently converge in the middle section through a distance weight feedback function; finally, a path secondary optimization module is developed, which uses turning angle screening and cubic Hermite interpolation technology to eliminate redundant turning points and generate a smooth trajectory that meets kinematic constraints. Experiments show that in a comprehensive scenario with semi-closed obstacles and irregular boundaries, compared with the classic A*, the method of the present invention significantly reduces the number of path search nodes, the average number of turns and the cumulative number of turns, and shortens the planning time. The algorithm significantly improves the planning efficiency and path executability in complex environments, and provides an effective solution for navigation systems in the fields of service robots, intelligent warehousing, etc. BRIEF DESCRIPTION OF THE DRAWINGS
[0069] Figure 1 It is a grid map diagram.
[0070] Figure 2 This is a schematic diagram of robot modeling.
[0071] Figure 3 This is a diagram of Manhattan distance.
[0072] Figure 4 This is a diagram of a redundant search caused by the bidirectional A* algorithm where the forward and backward paths do not intersect.
[0073] Figure 5 It is a schematic diagram of the intersection of the forward path and the backward path of the method of the present invention in the middle of the map.
[0074] Figure 6 This is a schematic diagram of the incremental grid list on the line connecting two points.
[0075] Figure 7 This is a schematic diagram of variable neighborhood search.
[0076] Figure 8 It is a schematic diagram of path polyline optimization.
[0077] Figure 9 This is the effect of path smoothing.
[0078] Figure 10 This is a schematic diagram of the path planned by the traditional A* algorithm in a map with regular obstacle shapes and relatively even distribution.
[0079] Figure 11 It is a schematic diagram of the path planned by the method of the present invention in a map with regular obstacle shapes and relatively uniform distribution.
[0080] Figure 12 This is a schematic diagram of the path planned by the traditional A* algorithm in a map with regular but unevenly distributed obstacles.
[0081] Figure 13 It is a schematic diagram of the path planned by the method of the present invention in a map where obstacles have regular shapes but uneven distribution.
[0082] Figure 14 This is a schematic diagram of the path planned by the traditional A* algorithm in a map with irregular obstacles.
[0083] Figure 15 It is a schematic diagram of the path planned by the method of the present invention in a map with irregular obstacles.
[0084] Figure 16 This is a schematic diagram of the path planned by the traditional A* algorithm in a strip-shaped obstacle map with turns.
[0085] Figure 17 It is a schematic diagram of the path planned by the method of the present invention in a strip obstacle map with turns.
[0086] Figure 18 This is the first set of experimental path diagrams of the traditional A* algorithm on a comprehensive map.
[0087] Figure 19 This is a schematic diagram of the first set of experimental paths of the method of the present invention in a comprehensive map.
[0088] Figure 20 This is a schematic diagram of the second set of experimental paths of the traditional A* algorithm on a comprehensive map.
[0089] Figure 21This is a schematic diagram of the second set of experimental paths of the method of the present invention in a comprehensive map.
[0090] Figure 22 This is a schematic diagram of the third set of experimental paths of the traditional A* algorithm on a comprehensive map.
[0091] Figure 23 This is a schematic diagram of the third set of experimental paths of the method of the present invention in the comprehensive map. DETAILED DESCRIPTION
[0092] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, and the above and / or other advantages of the present invention will become more apparent.
[0093] The present invention provides a comprehensive scenario map path planning method based on an improved A* algorithm, comprising the following steps:
[0094] Step 1: Conduct basic environment modeling for the comprehensive scene map;
[0095] As an algorithm based on graph search, A* requires map construction as an important pre-step for path planning. Grid map is the most commonly used map in A* algorithm. It divides the two-dimensional plane scene into multiple regular quadrilateral grids and assigns a value to each grid in a binary way to represent the occupancy of the grid. Figure 1 As shown, the value of the grid occupied by an obstacle (even if the obstacle is not completely present) is set to 1 and filled with black, while the value of the remaining grid is set to 0 and filled with white. Path planning generates a safe path within the white traversable area. As the map expands or the environment becomes more complex, the efficiency of raster map information maintenance will also decrease, so the setting of the unit grid size becomes critical: a larger grid setting speeds up the map information reading rate but may cause some smaller obstacles to be ignored; a smaller grid setting increases the map accuracy but also brings a heavy workload.
[0096] In order to ensure the safety of the path, the present invention constructs a protection area consisting of a single layer of grids around the obstacles, which is represented by gray grids in the map. Figure 2 As shown, the radius of the mobile robot's circumscribed circle is set to R, and the safe distance between it and obstacles is defined as r. In this invention, the safe distance r is used as the side length of the grid. After the path is optimized and smoothed, the path may become tangent to the corners of the protection area grid. However, this protective layer design effectively avoids collisions with actual obstacles even when the path is tangent to the protection grid.
[0097] Step 2: Use the improved A* algorithm to plan the path;
[0098] Step 2-1, establishing an improved bidirectional search strategy;
[0099] The traditional A* algorithm usually adopts a one-way search method from the starting point to the target point, which is less efficient when facing large maps with dense obstacles. To solve this problem, researchers have proposed a bidirectional A* (BA*) algorithm that combines a bidirectional search strategy with the A* algorithm. In BA*, the search starts from the starting point and the target point at the same time, and each regards the other as the target until the two paths meet in the middle. Then, backtrack from the meeting point to the starting point and the target point to form a complete path. In theory, compared with one-way search, BA* can reduce the search time by about 50%. However, this method also has certain limitations: the general BA* uses the Manhattan distance from the current path node to the starting point of the opposite path as the evaluation function, such as Figure 3 middle and However, when the two paths diverge in direction, the forward and backward paths become disjointed due to a lack of effective guidance for the opposite path. The number of nodes required for expansion may exceed that of the traditional one-way A* algorithm, consuming more computing resources.
[0100] This paper proposes an improved two-way search strategy by adjusting the distance evaluation function in the heuristic function. To optimize the search process, as shown in formula (1). Figure 3 As shown, when we first start to find the path, and exist and This can guide the node to quickly search towards the target point. As the pathfinding process progresses, The weight of the map gradually increases, guiding the paths on both sides to approach each other, so that the paths meet in the middle of the map.
[0101] (1) ,
[0102] Simplifying the above formula (1) yields formula (2):
[0103] (2).
[0104] The improved two-way search strategy of the present invention and the common two-way search strategy are compared in the same map. Figure 4 As shown in , the general two-way search strategy is difficult to deal with maps with complex obstacles and easily leads to redundant searches. Figure 5 As shown, the distance-improved strategy proposed in the present invention can better coordinate the search directions of the two paths, reduce path deviations, and improve search efficiency.
[0105] Step 2-2, establish a heuristic function with obstacle line density weight;
[0106] The A* algorithm expands the nodes in the map through the heuristic function value. When the heuristic function value is close to the actual estimated path length, the planned path often has a higher global optimality. The present invention proposes a new distance evaluation function in the improved two-way search strategy. However, when the evaluation function is simply used as a heuristic function, it is impossible to effectively avoid the influence of obstacle-dense areas in the map. In path planning, when the path between two points encounters an obstacle, it is usually necessary to detour to avoid the obstacle, which will increase the path length. The present invention designs an optimized heuristic function that combines obstacle density as an adjustment factor with the distance function to simulate the detour cost under different obstacle distribution environments.
[0107] Calculating the obstacle surface density requires traversing all grids in the rectangular area determined by the two points as diagonal vertices, which will increase the computational complexity of the heuristic function and thus may reduce the pathfinding speed. To this end, the present invention uses line density to determine the obstacle distribution density between the two-point path. The purpose of the present invention is to allow the two-side paths to converge in the map, so the present invention uses the current node of the forward path to be the same as the node of the forward path. and the current node of the backward path The obstacle line density between them is added as an adjustment factor to their heuristic functions. Figure 6 As shown, the Bresenham algorithm is first used to obtain the incremental grid list in the line connecting two points, and then the list is traversed to calculate the proportion of obstacle grids, thereby obtaining the obstacle density of the path.
[0108] Assume that the slope of the line connecting the forward path and the backward path is k:
[0109] (3),
[0110] in The current node of the backward path The vertical coordinate of is the current node of the forward path The vertical coordinate of The current node of the backward path The horizontal axis of Current node on the forward path The horizontal axis Indicates the increment in the horizontal direction; Indicates the increment in the vertical direction;
[0111] On the slope In the case of Larger, adjust the horizontal coordinate position of the corresponding node in steps of 1 in the horizontal direction, set and On the connected straight line The coordinates of the nodes are , the horizontal coordinate value on the straight line is The vertical coordinate value corresponding to the point is:
[0112] (4),
[0113] in, for and The horizontal coordinate on the straight line connected to The vertical coordinate value corresponding to the point; b is and The intercept of the connected straight line on the y-axis. This parameter will be offset in subsequent calculations;
[0114] calculate 、 as well as 、 The difference :
[0115] (5),
[0116] in, The coordinates are The grid to and The difference in ordinates of the connected straight lines, The coordinates are The grid to and The difference in ordinates of the connected straight lines, for and On the connected straight line The vertical coordinate value of the node; calculate the Decision parameters for the first step :
[0117] (6),
[0118] when hour , the point below the straight line is closer to the straight line. ;when hour , the point above the straight line is closer to the straight line. ,Will Substitute into formula (6) to find the decision parameter of step i+1 :
[0119] (7),
[0120] Use starting point The first node coordinates next to it are calculated to obtain the decision parameters of the first incremental grid :
[0121] (8),
[0122] get After that, the subsequent incremental grid decision parameters are deduced by formula (7), and the incremental grid decision parameters are used to determine the distribution of the incremental grid on the vertical coordinate until the current node of the backward path is encountered. , get the incremental grid list between the forward path and the backward path ;
[0123] Iterate over the incremental grid list Nodes in the grid, calculate the linear density of obstacles :
[0124] (9) ,
[0125] Among them, num(obstacle) is the number of obstacle nodes; num(node) is the number of all nodes.
[0126] Finally, combined with formula (2), we can get the heuristic function of the forward path: and the heuristic function of the backward path :
[0127] (10).
[0128] Increasing the obstacle line density as an adjustment factor makes the expected cost of the path closer to the actual value. When the distance function is equal, the node will preferentially expand to the area with fewer obstacles, which reduces the detour cost while increasing the safety of the path.
[0129] Step 2-3: Use the node expansion method of changing the neighborhood to complete the node expansion;
[0130] When expanding nodes, the traditional A* algorithm usually expands around the node in an even manner. However, when the search direction deviates from the target node, this even expansion may lead to a large number of unnecessary searches in invalid areas, thereby reducing the search efficiency. The classic 4-neighborhood search or 8-neighborhood search can only move forward in the horizontal, vertical or diagonal direction each time after traversing the node. Since its step size is too short, multiple traversals are required to complete the pathfinding. The present invention proposes a variable-domain node expansion method. Compared with the traditional 8-neighborhood search, this method discards nodes that are relatively far away from the target direction, and at the same time adds several search nodes in the area facing the target direction, such as Figure 7 shown.
[0131] Take the current node of the opposite path as the target point , with the current node Establish a local coordinate system for the origin, define the horizontal right direction as the positive direction of the x-axis and the vertical upward direction as the positive direction of the y-axis, and divide the position of the target point relative to the current node into eight cases, namely four different quadrant areas and four directions of the coordinate axis:
[0132] The first case is: target point Located in the first quadrant area;
[0133] The second case is: target point Located in the second quadrant area;
[0134] The third case is: target point Located in the third quadrant area;
[0135] The fourth case is: target point Located in the fourth quadrant area;
[0136] Fifth case: target point Located in the positive direction of the x-axis;
[0137] The sixth case: target point Located in the positive direction of the y-axis;
[0138] The seventh case: target point Located in the negative direction of the x-axis;
[0139] Case 8: Target point Located in the negative direction of the y-axis;
[0140] If the target point Relative to the current node The position is the first, second, third, and fourth cases: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to The other three nodes to be expanded in the same direction and the current node Expand 4 nodes in the up, down, left, and right directions, and expand 8 nodes in total; if the node If it is an obstacle node, only the current node Expand 4 nodes in the up, down, left, and right directions, for a total of 5 nodes;
[0141] If the position of the target point relative to the current node is the fifth, sixth, seventh, or eighth case: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to One node in the same direction around the current node In the up, down, left, and right directions, except for the nodes 3 nodes other than the one above are expanded, for a total of 5 nodes; if the node If it is an obstacle node, only the current node In the up, down, left, and right directions, except for the nodes Expand the other 3 nodes, and expand a total of 4 nodes.
[0142] This approach improves search accuracy and reduces the number of redundant nodes. Since increasing the single traversal step size can reduce the number of traversals, search efficiency is improved.
[0143] Steps 2-4: perform secondary path optimization, including:
[0144] Step 2-4-1, broken line optimization;
[0145] The initially planned path still has a lot of room for improvement. For some redundant turns in the path, the present invention will prune the path, delete unnecessary turning points in the path, reduce the total turning angle in the path, and ensure that the optimized path does not collide with obstacles.
[0146] The node pruning process is as follows Figure 8 As shown: First, the first node Add it to the optimized path list as the starting point, and judge the subsequent nodes in the original path in turn. Whether the connection line crosses the obstacle, after traversing the second node , the third node , the fourth node Afterwards, it was discovered With the fifth node If there is an obstacle between Previous node Add to the optimization path list and update Continue traversing the nodes in the original path for the new starting point. If With the ninth node If there are obstacles between Previous node Add the optimized path list and update the starting point. Repeat steps 2-4-1 until the end of the path is traversed. Finally, add the end point to the optimized path list.
[0147] like Figure 8 As shown in the figure, after pruning, the number of turning points of the path is reduced from 4 to 2, and the turning angle and path length are significantly reduced.
[0148] Step 2-4-2, path smoothing;
[0149] In order to make the path more suitable for the mobile robot and reduce the robot's motion loss at turning points, the present invention uses piecewise cubic Hermite interpolation to smooth the path. Compared with traditional cubic spline interpolation, it has stronger monotonicity, is less prone to oscillation, and can handle paths with more turning points.
[0150] Set in the interpolated path discrete nodes ,in for The discrete nodes The horizontal coordinate of each discrete node; For the The horizontal coordinates of discrete nodes The corresponding vertical coordinate is then defined as follows for the data spacing and normalization parameters between adjacent nodes:
[0151] (11),
[0152] in: is the length of the i-th interpolation interval; z is the horizontal coordinate of the interpolation to be calculated; t is the normalization parameter, and the interval Mapping to intervals ;
[0153] Next, calculate the average gradient of each interval:
[0154] (12),
[0155] in ;
[0156] In order to avoid oscillation during interpolation and maintain monotonicity, the node derivatives are also determined at each interpolation node. ,if and Different signs, then the derivative Set to 0; if and If they have the same sign, they are calculated as follows:
[0157] (13),
[0158] So far, in each interval Construct the third piecewise cubic Hermite interpolation polynomial:
[0159] (14),
[0160] in Indicates the Cubic interpolation polynomial on the segment; 、 Respectively represent The gradient value at the interpolation node and the The gradient value at the interpolation node; through the above steps, the entire path can be smoothly interpolated while maintaining the continuity of the function value and derivative at the node. The smoothed path is as follows Figure 9 shown.
[0161] In this embodiment, simulation and analysis are performed in various scenarios, including:
[0162] 1. Algorithm performance evaluation based on different types of obstacles;
[0163] In order to evaluate the algorithm's versatility and adaptability to the environment, the present invention designed four maps with different obstacle types for simulation. Each map represents the characteristics of a real scene. Considering that mobile robots usually work in relatively spacious areas, the four maps of the present invention are all set to large scene maps of size 100*100. The starting and ending points of all scenes are in the lower left corner. To the upper right The simulation experiment of the present invention was carried out in MATLAB 2018b, and the recorded path planning time was the average time of 30 repeated experiments.
[0164] (1) For a map with regular obstacle shapes and relatively even distribution, the path planned by the traditional A* algorithm is as follows: Figure 10 As shown; the path planned by the method of the present invention is as follows Figure 11 As shown, the experimental data are recorded in Table 1.
[0165] Table 1
[0166]
[0167] The obstacles in this map are all relatively conventional rectangular obstacles, distributed fairly evenly across the map, simulating a typical environment. The traditional A* algorithm uses an 8-neighborhood search to expand nodes, expanding more nodes on both sides of a straight path. However, this invention uses an improved variable-neighbor search strategy that discards nodes that stray from the target point during the search, reducing the number of nodes expanded. The red node areas on both sides of the straight line are significantly thinner than those in the A* algorithm, reducing the node expansion rate by 42.857%.
[0168] (2) For maps with regular obstacle shapes but uneven distribution, the traditional A* algorithm plans the path as follows: Figure 12 As shown; the path planned by the method of the present invention is as follows Figure 13 The experimental data are recorded in Table 2.
[0169] Table 2
[0170]
[0171] Figure 13 The obstacle density in the map decreases from the diagonal line to both sides. The traditional A* algorithm moves in a 45° direction to the upper right when planning the path, and only begins to adjust the path direction when encountering obstacles. Because the obstacle line density adjustment factor is added to the heuristic function of the algorithm of this invention, the detour cost can be better evaluated. The paths from the starting point and the end point are extended to the area with low obstacle density at the path nodes in the early planning stage, which reduces the cost of the path passing through obstacles and reduces the turning angle by 69.232%.
[0172] (3) Maps with irregular obstacles, traditional A* algorithm plans paths such as Figure 14 As shown; the path planned by the method of the present invention is as follows Figure 15 The experimental data are recorded in Table 3.
[0173] Table 3
[0174]
[0175] The traditional A* algorithm generates a jagged path when avoiding irregular obstacles in the map. Due to the addition of a broken line optimization strategy, the path generated by the algorithm of the present invention is significantly smoother, with the number of turns reduced by 65.217% and the turning angle reduced by 84.398% compared to the traditional A*.
[0176] (4) Obstacle map with turns in strips, the traditional A* algorithm plans the path as follows Figure 16 As shown; the path planned by the method of the present invention is as follows Figure 17 The experimental data are recorded in Table 4.
[0177] Table 4
[0178]
[0179] The obstacles in the map have corners. The traditional A* algorithm is stuck in the corner area and performs redundant searches, which also consumes a lot of search time. The improved two-way search strategy adopted by the algorithm of this invention avoids this problem near the end point. Due to the mutual attraction of the paths on both sides, the paths successfully intersect in the middle of the map, the planning efficiency of the algorithm is significantly improved, and the planning time is reduced by 67.621% compared with the traditional A* algorithm.
[0180] Analysis of simulation results for the four characteristic obstacle maps shows that the proposed algorithm not only demonstrates excellent planning efficiency in conventional maps but also effectively handles irregular obstacles and prioritizes detours toward areas with low obstacle density in maps with uneven obstacle distribution. The proposed algorithm is particularly advantageous when complex obstacles are present near the endpoint. It outperforms the traditional A* algorithm in terms of path length, smoothness, and planning efficiency.
[0181] 2. Simulation analysis based on real scene maps;
[0182] After analyzing the algorithm's ability to circumvent several typical obstacle types, we simulated the algorithm on a comprehensive map. Since obstacles in real-world scenarios are more diverse and maps are increasingly complex, we rasterized the home interior scene, also at a 100*100 pixel size.
[0183] The obstacles in the map are unevenly distributed and contain a variety of irregular obstacles and semi-enclosed spaces. In this map, the present invention selects three sets of starting points and target points that are far apart to analyze the planning ability of the algorithm in a comprehensive large-scale scene map. The data is recorded in Table 5. In the first set of experiments, the path of the traditional A* algorithm is as follows Figure 18 As shown, the path of the method of the present invention is as follows Figure 19 As shown; in the second set of experiments, the path of the traditional A* algorithm is as follows Figure 20 As shown, the path of the method of the present invention is as follows Figure 21 As shown; in the third group of experiments, the path of the traditional A* algorithm is as follows Figure 22 As shown, the path of the method of the present invention is as follows Figure 23 shown.
[0184] Table 5
[0185]
[0186] The above three groups of experiments were analyzed in turn. In the first group of experiments, the improved A* algorithm was similar to the traditional A* algorithm in length, but the number of turns and angles were significantly reduced, the number of expanded nodes was smaller, and the time was relatively short. This is because the variable step size strategy and adaptive heuristic function reduced unnecessary searches, thereby reducing node expansion and saving time. The small number of turns is due to quadratic optimization and Hermite interpolation. In the second group of experiments, the improved A* path length was slightly longer than the classic A*, but better than other improved algorithms. This is because the obstacle density near the target point of the path varies greatly, and the planned path tends to develop in areas with fewer obstacles, resulting in a slight extension of the path, but the overall path quality of the algorithm of the present invention is better. In the third group of experiments, the path length and path smoothness of the improved A* algorithm were both the best results.
[0187] Research has confirmed that the innovative architecture that integrates directional search and path smoothing strategies can balance the contradiction between path optimality and computational real-time performance. The present invention proposes an improved A* algorithm suitable for comprehensive scene maps. Based on the traditional A* algorithm, this method introduces a variable-step node expansion method and improves the two-way search strategy, and performs secondary optimization and Hermite smoothing operations on the path to improve the continuity of the path. The obstacle avoidance capability of the algorithm compared to the traditional A* was verified in four typical obstacle maps, and then the superiority of the algorithm of the present invention in planning efficiency and path quality was verified by comparison with the traditional A* algorithm in a comprehensive map: the number of expanded nodes was greatly reduced, the number of path turns and angles were greatly reduced, and the planning time was also reduced accordingly. The current experiments are all based on simulations. In the future, it will be considered to continue to improve the algorithm of the present invention in actual robots, and dynamic obstacle scenes will be considered and expanded in combination with the rolling window method.
[0188] This invention provides a comprehensive scenario map path planning method based on an improved A* algorithm. There are many methods and approaches to implement this technical solution. The above is only a preferred embodiment of the invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the invention, and such improvements and modifications should also be considered within the scope of protection of this invention. Any components not specified in this embodiment may be implemented using existing technologies.
Claims
1. A comprehensive scene map path planning method based on an improved A* algorithm, characterized in that: The following steps are involved: Step 1: Perform basic environment modeling for the comprehensive scene map: Divide the two-dimensional plane scene into two or more regular quadrilateral grids, and assign values to each grid in a binary manner to represent the grid occupancy status. The value of the grid occupied by the obstacle is set to 1 and filled with black, and the value of the remaining grid is set to 0 and filled with white. Construct a protection zone consisting of a single layer of grids around the obstacle and fill it with gray. Set the radius of the mobile robot's circumscribed circle to R, and the safe distance between the mobile robot and the obstacle to r, with r as the side length of the grid. Step 2: Use the improved A* algorithm to plan the path, including the following steps: Step 2-1, establishing an improved bidirectional search strategy; Step 2-2, establish a heuristic function with obstacle line density weight; Step 2-3: Use the node expansion method of changing the neighborhood to complete the node expansion; Steps 2-4, perform secondary path optimization; Step 2-1 includes: the formula of the improved bidirectional search strategy is: (1) ， in, Indicates the starting point The current node of the forward search path The distance between the evaluation function value, Indicates the target point To the current node of the backward search path The distance evaluation function value between them; d represents the current node of the forward search path The current node of the backward search path The Manhattan distance between Represents the current node of the forward search path With the target point The Manhattan distance between Represents the current node of the backward search path and starting point The Manhattan distance between Step 2-1 also includes: simplifying formula (1) to obtain: (2)。 2. The method according to claim 1, characterized in that Step 2-2 includes: The current node of the forward path and the current node of the backward path The obstacle line density between is added as a regulating factor to the heuristic function. First, the Bresenham algorithm is used to obtain and Connect the incremental grid list in the line, then traverse the incremental grid list to calculate the proportion of obstacle grids, so as to obtain the obstacle density of the path; Assume that the slope of the line connecting the forward path and the backward path is k: (3)， in The current node of the backward path The vertical coordinate of is the current node of the forward path The vertical coordinate of The current node of the backward path The horizontal axis of Current node on the forward path The horizontal axis of Indicates the increment in the horizontal direction; Indicates the increment in the vertical direction; On the slope In the case of Larger, adjust the horizontal coordinate position of the corresponding node in steps of 1 in the horizontal direction, set and On the connected straight line The coordinates of the nodes are , the horizontal coordinate value on the straight line is The vertical coordinate value corresponding to the point is: (4)， in, for and The horizontal coordinate on the straight line connected to The vertical coordinate value corresponding to the point; b is and The intercept of the straight line connecting .
3. The method according to claim 2, characterized in that Step 2-2 also includes: calculate 、 as well as 、 The difference : (5)， in, The coordinates are The grid to and The difference in ordinates of the connected straight lines, The coordinates are The grid to and The difference in ordinates of the connected straight lines, for and On the connected straight line The vertical coordinate value of the node; calculate the Decision parameters for the first step : (6)， in Indicates the increment in the horizontal direction; Indicates the vertical increment; when hour , the point below the straight line is closer to the straight line. ;when hour , the point above the straight line is closer to the straight line. ,Will Substitute into formula (6) to find the decision parameter of step i+1 : (7)， Use starting point The first node coordinates next to it are calculated to obtain the decision parameters of the first incremental grid : (8)， get After that, the subsequent incremental grid decision parameters are deduced by formula (7), and the incremental grid decision parameters are used to determine the distribution of the incremental grid on the vertical coordinate until the current node of the backward path is encountered. , get the incremental grid list between the forward path and the backward path .
4. The method according to claim 3, characterized in that Step 2-2 also includes: Iterate over the incremental grid list Nodes in the grid, calculate the linear density of obstacles : (9) ， Among them, num(obstacle) is the number of obstacle nodes; num(node) is the number of all nodes; Finally, combined with formula (2), we can get the heuristic function of the forward path: and the heuristic function of the backward path : (10)。 5. The method according to claim 4, characterized in that Step 2-3 includes: taking the current node of the opposite path as the target point , establish a local coordinate system with the current node o as the origin, define the horizontal rightward direction as the positive direction of the x-axis and the vertical upward direction as the positive direction of the y-axis, and divide the position of the target point relative to the current node into eight cases, namely four different quadrant areas and four directions of the coordinate axis: The first case is: target point Located in the first quadrant area; The second case is: target point Located in the second quadrant area; The third case is: target point Located in the third quadrant area; The fourth case is: target point Located in the fourth quadrant area; Fifth case: target point Located in the positive direction of the x-axis; The sixth case: target point Located in the positive direction of the y-axis; The seventh case: target point Located in the negative direction of the x-axis; Case 8: Target point Located in the negative direction of the y-axis; If the target point Relative to the current node The position is the first, second, third, and fourth cases: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to The other three nodes to be expanded in the same direction and the current node Expand 4 nodes in the up, down, left, and right directions, and expand 8 nodes in total; if the node If it is an obstacle node, only the current node Expand 4 nodes in the up, down, left, and right directions, for a total of 5 nodes; If the position of the target point relative to the current node is the fifth, sixth, seventh, or eighth case: first expand the target point The distance from the current node in the direction The most recent node , judgment node Is it an obstacle? If the node If it is not an obstacle node, continue to One node in the same direction around the current node In the up, down, left, and right directions, except for the nodes 3 nodes other than the one above are expanded, for a total of 5 nodes; if the node If it is an obstacle node, only the current node In the up, down, left, and right directions, except for the nodes Expand the other 3 nodes, and expand a total of 4 nodes.
6. The method according to claim 5, characterized in that Steps 2-4 include: Step 2-4-1, broken line optimization; Prune the path: First, the first node Add it to the optimized path list as the starting point, and judge the subsequent nodes in the original path in turn. Whether the connection line crosses the obstacle, after traversing the second node , the third node , the fourth node Afterwards, it was discovered With the fifth node If there is an obstacle between Previous node Add to the optimization path list and update As the new starting point, continue to traverse the nodes in the original path. If you find With the ninth node If there are obstacles between Previous node Add the optimized path list and update the starting point. Repeat steps 2-4-1 until the end of the path is reached. Finally, add the end point to the optimized path list. Step 2-4-2, use piecewise cubic Hermite interpolation to smooth the path: set the path after interpolation to have discrete nodes ,in for The horizontal coordinate of the jth discrete node among discrete nodes; is the horizontal coordinate of the jth discrete node The corresponding vertical coordinate is then defined as follows for the data spacing and normalization parameters between adjacent nodes: (11)， in: is the length of the i-th interpolation interval; z is the horizontal coordinate of the interpolation to be calculated; t is the normalization parameter, and the interval Mapping to intervals ; Next, calculate the average gradient of each interval: (12)， in ; At each interpolation node, the node derivative must also be determined ,if and Different signs, then the derivative Set to 0; if and If they have the same sign, they are calculated as follows: (13)， So far, in each interval Construct the third piecewise cubic Hermite interpolation polynomial: (14)， in represents the cubic interpolation polynomial on the jth segment; 、 Represent the gradient value at the jth interpolation node and the The gradient value at the interpolation node.
7. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores program codes, and when the program codes are executed by the processor, the processor is caused to perform the steps of the method according to any one of claims 1 to 6.
8. A storage medium, characterized in that: A computer program or instruction is stored, and when the computer program or instruction is run on a computer, the steps of the method according to any one of claims 1 to 6 are executed.