Robot path planning method based on dynamic heuristic function

Abstract

The invention discloses a robot path planning method based on a dynamic heuristic function, and the method comprises the steps: S1, initializing a binary grid map, and generating a random obstacle; s2, algorithm parameters are set, a list is opened, list initialization is closed, and a path planning starting point and a path planning ending point are set; s3, expanding the current node, and obtaining 8-direction adjacent nodes; s4, continuously calculating the obstacle density and calculating a dynamic weighted heuristic function based on the Chebyshev distance; s5, updating the node cost, judging whether the path is a better path, if the path is the better path, adding the path to the open list, and updating the father node relationship; and S6, calculating the path cost until a global optimal path is obtained, and carrying out path smoothing processing on the global optimal path. The problems of heuristic function fixed weight and a large number of invalid search areas can be solved, sharp turning points of the path can be reduced, the path feasibility is realized, and the mechanical loss of the robot is reduced.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control and path planning, and in particular relates to a robot path planning method based on a dynamic heuristic function. Background Art

[0002] In industrial logistics and intelligent warehousing, with the development of robotics technology, path planning, as the core of robot navigation, has profound application value. The traditional A* algorithm, as a widely adopted global path planning algorithm, uses heuristic functions to guide the search direction, ensuring that even in complex environment settings, it can obtain the global optimal path with minimal computing time. However, in complex maps, the traditional A* algorithm uses heuristic functions for evaluation and utilizes Manhattan distance or Euclidean distance. It faces problems such as limited mobility, redundant search nodes, long search time and a large number of grids traversed, only suitable for simple scenarios, and low computational efficiency. At the same time, improved algorithms based on the A* algorithm also face problems such as the use of fixed heuristic function weights, path length fluctuations, over-reliance on heuristic functions resulting in reduced efficiency, and a large amount of invalid search space. Summary of the Invention

[0003] To solve the above technical problems, the present invention proposes a robot path planning method based on dynamic heuristic function, which reduces sharp turning points of the path, achieves path feasibility, and reduces mechanical loss of the robot.

[0004] To achieve the above object, the present invention provides a robot path planning method based on a dynamic heuristic function, comprising:

[0005] Initialize the binary grid map and generate random obstacles;

[0006] Set algorithm parameters, open and close list initialization, and set path planning start and end points;

[0007] Expand the current node and obtain the adjacent nodes in 8 directions;

[0008] Continuously calculate obstacle density and dynamically weighted heuristic functions based on Chebyshev distance;

[0009] Update the node cost and determine whether it is a better path. If it is a better path, add it to the open list and update the parent node relationship;

[0010] Calculate the path cost until a global optimal path is obtained, and perform path smoothing on the global optimal path.

[0011] Technical effect of the present invention: The present invention discloses a robot path planning method based on a dynamic heuristic function. The heuristic function is based on the Chebyshev distance and obstacle density, and can perform fast path planning under complex maps to meet the application requirements of real-time path planning. At the same time, eight-direction search and Chebyshev distance are used to overcome the problems of limited mobility and redundant nodes, and obstacle density is considered. Through this dynamic heuristic function, the robot can stay away from high-risk and high-obstacle density areas, and can solve the problems of fixed weights of the heuristic function and a large number of invalid search areas. For the generated optimal path, through smoothing processing, the sharp turning points of the path can be reduced, the path feasibility can be achieved, and the mechanical loss of the robot can be reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] The accompanying drawings, which constitute part of this application, are intended to provide a further understanding of this application. The exemplary embodiments and descriptions of this application are intended to explain this application and do not constitute an improper limitation on this application. In the accompanying drawings:

[0013] Figure 1 Schematic diagram of the overall algorithm framework of a robot path planning method based on a dynamic heuristic function according to an embodiment of the present invention;

[0014] Figure 2 The figure is a flow chart of a robot path planning method based on a dynamic heuristic function according to an embodiment of the present invention. DETAILED DESCRIPTION

[0015] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0016] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0017] like Figure 1 and Figure 2 As shown, this embodiment provides a robot path planning method based on a dynamic heuristic function, including:

[0018] Step 1: Initialize the binary grid map and generate random obstacles;

[0019] Step 2: Set the algorithm parameters, open and close the list initialization, and set the path planning start and end points;

[0020] Step 3: Expand the current node and obtain the adjacent nodes in 8 directions;

[0021] Step 4: Continuously calculate obstacle density and dynamically weighted heuristic function calculation based on Chebyshev distance;

[0022] Step 5: Update the node cost and determine whether it is a better path. If it is a better path, add it to the open list and update the parent node relationship;

[0023] Step 6: Calculate the path cost until the global optimal path is obtained, and perform path smoothing on the global optimal path.

[0024] Furthermore, in step 1, a two-dimensional grid map is constructed, obstacles are randomly generated, and 1 is used to represent an inaccessible area, i.e., the area where the obstacle is located, and 0 is used to represent a passable area.

[0025] Specifically, in the embodiment of the present invention, a 20x20 grid map is initialized, obstacles are randomly generated, and 1 represents the area where the obstacle is located, and 0 represents the passable area.

[0026] Furthermore, in step 2, the open list and the closed list are initialized according to the map scale, and the algorithm parameters are set, including the density radius R, the exponential decay coefficient λ, and the density sensitivity coefficient β, and the path planning start and end points are set.

[0027] Specifically, in this embodiment of the present invention, based on the current map scale, the density radius R is set to 2, the attenuation coefficient β is set to 2.0, and the density sensitivity coefficient λ is set to 0.5. For maps of other scales, such as a 40x40 grid map, the parameters that can be used are R = 3, λ = 0.6, and β = 3.0; for a 60x60 grid map, the corresponding parameters are R = 4, λ = 0.7, and β = 4.0. For larger-scale maps, λ and β increase linearly by coefficients of 0.1 and 1.0, and R increases linearly by a coefficient of 1. The starting and end points of the path planning are set according to the task requirements. In this example, the starting point is set to (19, 0) and the end point is set to (0, 19).

[0028] Furthermore, in step 3, a path search is performed to iterate and determine whether the current node is the end point of the path. If the end point of the path has not yet been reached, the Chebyshev distance is used to obtain the adjacent nodes in 8 directions. The formula is as follows:

[0029] Distance(n) Chebyshev =max(|x n -x G |,|y n -y G |);

[0030] Among them, Distance(n) Chebyshev is the Chebyshev distance value of the nth data point; n is the number of the target point currently being calculated; x nis the x-axis position of point n in two-dimensional space; x G is the horizontal coordinate of the reference point G; n is the y-axis position of point n in two-dimensional space; G is the vertical coordinate of the reference point G.

[0031] Furthermore, in step 4, the normalized obstacle density D(n) of n nodes is calculated. The obstacle density is calculated within the area with a radius of R. The formula is as follows:

[0032]

[0033] Where D(n) is the normalized obstacle density of n nodes; R is the radius; total R is the total number of grids within the range radius R; obstacles is the number of obstacles in the area, and the neighborhood is defined using Manhattan distance. The Manhattan distance formula is as follows:

[0034] Distance(S, T) Manhattan =|x s -x T |+|y s -y T |;

[0035] Among them, Distance(S, T) Manhattan is the Manhattan distance between the starting point and the target point; S is the starting point; T is the target point; x s is the x-axis position of point S in the two-dimensional coordinate system; T is the x-axis position of point T in the two-dimensional coordinate system; y s is the y-axis position of point S in the two-dimensional coordinate system; T is the y-axis position of point T in the two-dimensional coordinate system.

[0036] The present invention proposes a dynamic heuristic function based on obstacle density and Chebyshev distance as follows:

[0037] h(n)=max(|x n -x G |,|y n -y G |)*(1+λ*e β*D(n) );

[0038] Among them, h(n) is the heuristic function; x n is the x-axis position of point n in two-dimensional space; x G is the horizontal coordinate of the reference point G; n is the y-axis position of point n in two-dimensional space; G is the vertical coordinate of the reference point G; λ is the density sensitivity coefficient; e β*D(n)is an exponential correction term; β is a nonlinear exponential attenuation coefficient; D(n) represents the normalized obstacle density, which is the obstacle density in the area near the n node (with a radius of R), D(n)∈[0,1]. When the robot moves to an area with high obstacle density, D(n) increases significantly, guiding the robot away from the area with high obstacle density. When D(n) approaches 0, the heuristic function uses the Chebyshev distance to maintain search efficiency.

[0039] Furthermore, in step 5, the algorithm uses the following heuristic function to calculate the cost of the current node, taking into account the path security and path length. g(n) is the path length cost from the starting node to the current node. If the node cost is smaller, the currently generated path is the better path. The node is added to the open list and the parent node relationship is updated.

[0040] f(n)=h(n)+g(n);

[0041] Among them, f(n) is the cost function of the current node n; h(n) is the estimated cost from the current node n to the target node; g(n) is the path length cost from the starting node to the current node n.

[0042] Furthermore, in step 6, the algorithm iterates continuously, using dynamic heuristic functions to guide the robot's movement, keeping it away from high-risk, high-obstacle areas and moving toward the final node until the global optimal path is obtained. The global optimal path is then smoothed using a cubic Bezier curve. The cubic Bezier curve can accommodate more complex smoothing paths, and the formula is as follows:

[0043] B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3;

[0044] Among them, B(t) represents the exact position of the corresponding point on the Bezier curve when the parameter is t, and t is the curve parameter; P0 is the starting control point; P1 is the first control point; P2 is the second control point; and P3 is the end control point.

[0045] On the optimal path, take four control points P0 corresponding to the starting point, P1, P2, and P3 corresponding to the end point. Decompose the cubic Bezier curve equation as follows:

[0046]

[0047] Where t∈[0,1], the starting point P0 corresponds to t=0, and the end point P3 corresponds to t=1. The cubic Bezier curve passes through the starting point P0 and the end point P3 and satisfies the convex hull property, which means that when the control points avoid obstacles, the path can also be guaranteed to be safe.

[0048] This invention discloses a robot path planning method based on a dynamic heuristic function. This heuristic function, based on the Chebyshev distance and obstacle density, enables rapid path planning in complex maps, meeting the application requirements of real-time path planning. Furthermore, eight-directional search and the Chebyshev distance are utilized to overcome the problems of restricted mobility and redundant nodes. The consideration of obstacle density is also proposed. This dynamic heuristic function allows the robot to avoid high-risk, high-obstacle density areas, and addresses the issues of fixed heuristic weights and a large number of invalid search areas. Smoothing the generated optimal path reduces sharp turning points, ensuring path feasibility and minimizing mechanical wear on the robot.

[0049] The above are merely preferred embodiments of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A robot path planning method based on dynamic heuristic function, characterized in that: include: S1, initialize the binary grid map and generate random obstacles; S2. Set algorithm parameters, open and close list initialization, and set path planning start and end points; S3. Expand the current node and obtain the adjacent nodes in 8 directions; S4, continuously calculates obstacle density and calculates a dynamic weighted heuristic function based on Chebyshev distance; S5. Update the node cost and determine whether it is a better path. If it is a better path, add it to the open list and update the parent node relationship; S6. Calculate the path cost until a global optimal path is obtained, and perform path smoothing processing on the global optimal path.

2. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: The algorithm parameters include density radius, exponential decay coefficient, and density sensitivity coefficient.

3. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: Obtaining 8-directional adjacent nodes includes: Distance(n) chebyshev =max(|x n -x G |,|y n -y G |); Among them, Distance(n) Chebyshev is the Chebyshev distance value of the nth data point; n is the number of the target point currently being calculated; x n is the x-axis position of point n in two-dimensional space; x G is the horizontal coordinate of the reference point G; n is the y-axis position of point n in two-dimensional space; G is the vertical coordinate of the reference point G.

4. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: Calculating obstacle density includes: Where D(n) is the normalized obstacle density of n nodes; R is the radius; total R is the total number of grids within the range radius R; obstacles is the number of obstacles in the area.

5. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: The calculation of the dynamically weighted heuristic function based on Chebyshev distance includes: h(n)=max(|x n -x G |,|y n -y G |)*(1+λ*e β *D (n) ); Among them, h(n) is the heuristic function; x n is the x-axis position of point n in two-dimensional space; x G is the horizontal coordinate of the reference point G; n is the y-axis position of point n in two-dimensional space; G is the vertical coordinate of the reference point G; λ is the density sensitivity coefficient; e β*D(n) is the exponential correction term; β is the exponential decay coefficient; D(n) is the normalized obstacle density of n nodes.

6. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: Additions to the open list include: f(n)=h(n)+g(n); Among them, f(n) is the cost function of the current node n; h(n) is the estimated cost from the current node to the target node; g(n) is the path length cost from the starting node to the current node.

7. The robot path planning method based on dynamic heuristic function according to claim 1, characterized in that: Performing path smoothing processing on the global optimal path includes: B(t)=(1-t) 3 P0+3t(1-t) 2 P1+3t 2 (1-t)P2+t 3 P3; Among them, B(t) represents the exact position of the corresponding point on the Bezier curve when the parameter is t, and t is the curve parameter; P0 is the starting control point; P1 is the first control point; P2 is the second control point; and P3 is the end control point.

Images