Improved A*path planning method based on driver guidance and dynamic map segmentation

Abstract

The invention belongs to the field of robot path planning. The invention provides an improved A * path planning method based on driver guidance and dynamic map segmentation, and aims to solve the problems that an existing path planning method is high in calculation complexity and insufficient in practicability in complex terrains and large-scale maps. A Bezier curve is adopted to fit a driver guiding path, and a dynamic map cutting method is designed based on the path, so that the calculation burden of useless areas is effectively reduced. And comprehensively considering the cost of the path length, the energy consumption and the path risk, optimizing the actual cost function and the heuristic function of the A * algorithm, and smoothing the planned path by adopting a cubic spline curve. The method can improve the efficiency and practicability of path planning.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to an improved A* path planning method based on driver guidance and dynamic map segmentation. Technical Background

[0002] With the development of autonomous driving and robotics technology, the application of path planning in complex terrains has gradually increased. At present, traditional path planning algorithms are inefficient and difficult to meet real-time requirements when dealing with large-scale maps with different terrain complexity and obstacle density. Traditional path planning methods are difficult to meet the path requirements of robots under different task conditions. Therefore, in recent years, many studies have focused on improving the efficiency, reliability and practicality of path planning algorithms in complex environments. However, research on path planning in situations where it is necessary to balance path length, energy consumption and path risk is still lacking. Summary of the Invention

[0003] In order to address the deficiencies of the prior art, the present invention provides an improved A* path planning method based on driver guidance and dynamic map segmentation, the method comprising the following steps:

[0004] Step 1: Preprocessing of the elevation grid map. First, based on the robot's motion performance limit, the elevation grid map is divided into feasible areas and infeasible areas through color mapping to achieve visualization effects. The map is divided into several small areas Ω i , a terrain complexity calculation method is proposed, through the slope S of each area i , obstacle density D i and terrain roughness R i Calculate the terrain complexity T of each area Ωi,complexity ;

[0005] Step 2: Through the human-computer interaction interface, calibrate the starting position and target position of the path planning on the map, and input a series of path guidance points based on the driver's intuitive judgment, and fit the driver's guidance path P through the Bezier curve. human ;

[0006] Step 3: Design a map segmentation method. Combined with the driver guidance path obtained in step 2, the map is segmented vertically using the path as the baseline. An adaptive map width adjustment algorithm is designed based on the terrain complexity to generate a segmented map.

[0007] Step 4: Design quantitative evaluation methods for path length, energy consumption, and path risk cost respectively, and make targeted improvements to the actual cost function and heuristic function of the A* algorithm based on the above methods. Finally, use a cubic spline curve to smooth the planned path to obtain a smooth path that meets the task requirements.

[0008] Furthermore, the elevation grid map preprocessing described in step 1 is characterized in that: based on the motion performance limit of the hexapod robot, a maximum slope threshold s of the grid terrain that the hexapod robot can pass through is set. t and terrain height threshold h t ,classify the grids with values ​​greater than the threshold as non-drivable areas, and visualize the feasible and infeasible areas through color mapping;

[0009] Divide the global map space into several regions Ω i , proposed a terrain complexity calculation method, by calculating S i 、D i and R i To calculate the terrain complexity of each area where α S , α D , α R is the weight of each terrain, which is adjusted according to the robot's sensitivity to different terrains. I represents the robot's sensitivity to terrain indicators, ranging from [0,1]. S =I S / (I S +I D +I R ), α D =I D / (I S +I D +I R ), α R =I R / (I S +I D +I R ).

[0010] Furthermore, the driver guidance path described in step 2: through the human-computer interaction interface, the starting position of the path planning is marked on the map (x start ,y start ) and target position (x goal ,y goal ), and input a series of path guidance points P based on the driver's intuitive judgment index ={(x1,y1),(x2,y2),…,(x n ,y n )}, Bezier curve By calculating B(t), the smooth path P can be fitted human , which is the set of points of all map grids that the curve passes through, where t∈[0,1],

[0011] Furthermore, the map cutting method described in step 3 generates a segmented map, characterized in that: according to the driver's guidance path Phuman The map grid determines a series of grid points (x j ,y j ), which is located in the region Ω i When taking w in the direction perpendicular to the path i / 2 distance, the covered area is part of the search map, w i Determined by the width adaptive adjustment algorithm, as shown in formula (1), T max is the upper limit of the terrain complexity of the pre-set area, and w0 is the basic width of the pre-set segmented map; Calculate the unit vector perpendicular to the path direction As shown in formula (2): The boundary point of the search map corresponding to this point is determined as shown in formula (3).

[0012] Furthermore, the quantitative evaluation method of path length, energy consumption and path risk cost in step 4 and the targeted improvement of the actual cost function and heuristic function in the A* algorithm are characterized by: the path length cost is the total length of the path, which is directly calculated as the accumulation of all node distances, as shown in formula (4) d k is the distance from node k-1 to node k;

[0013] The energy consumption cost is the energy consumption calculation formula of the robot under different slope and terrain conditions as shown in formula (5):

[0014] Where m is the mass of the robot, g is the acceleration due to gravity, and θ k is the slope of the kth path, f is the dynamic efficiency factor, μ friction is the friction coefficient between the robot and the ground;

[0015] The path risk cost is measured by the terrain inclination, terrain complexity, and the deviation between the planned path and the driver's guided path during the robot's driving process. It is believed that in complex terrain, the higher the complexity of the terrain, the closer the planned path is to the driver's guided path, and the lower the risk. The calculation formula is shown in formula (6): is the terrain complexity of region i, d deviation,k is the offset distance between the kth path and the driver's guidance path, d MAX 1 / 2 map width w of region i i / 2,θ kis the current terrain slope, θ MAX is the maximum terrain slope s that the robot can pass t , r and q are nonlinear factors, both greater than 1;

[0016] The offset distance d deviation,k It can be calculated by the shortest distance between the actual path point of the robot and the grid point of the guide path, as shown in formula (7), where P human is the grid point set that the driver’s guidance path passes through, p robot,k is the coordinate of the robot on the kth path;

[0017] In summary, the comprehensive cost formula is shown in formula (8): C=ω E ·E+ω D ·D+ω S ·S (8)

[0018] The improvements to the heuristic function and actual cost function of the A* algorithm are shown in formulas (9) and (10), respectively. g(n) represents the cumulative cost from the starting point to node n, which is a comprehensive cost of energy consumption, path risk, and path length. h(n) represents the expected cost from the current node to the target location. g(n)=g(n parent )+C(n parent ,n) (9) The expected energy consumption cost Expected path length cost Expected risk cost As shown in formula (11), (12), and (13), respectively, is the straight-line distance from the current node to the target node; where ω E ,ω D ,ω S It is the weight factor of each cost, which is used to adjust the impact of different costs on the total cost. Its value will be determined by fuzzy rules based on the requirements of different tasks for the path.

[0019] Beneficial effects of the present invention:

[0020] This paper proposes an improved A* path planning method based on driver guidance and dynamic map segmentation. By combining driver guidance and dynamic map segmentation to reduce the path search range, a comprehensive cost function for path length, energy consumption, and path risk is designed. Fuzzy rules are used to determine the weights of these costs. This method can generate safe and efficient driving paths based on the path requirements of different tasks. This improves planning efficiency and feasibility, ensuring the robot's stability and task execution efficiency in complex terrain. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 Flowchart of robot multi-target path planning with map segmentation for driver guidance

[0022] Figure 2 Schematic diagram of the map cutting algorithm

[0023] Figure 3 To improve the A* algorithm principle diagram

[0024] Figure 4 is the fuzzy rule table DETAILED DESCRIPTION

[0025] In order to make the above-mentioned objects, features and advantages of the present invention clear and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0026] like Figure 1 As shown, the present invention provides an improved A* path planning method based on driver guidance and dynamic map segmentation, the method comprising the following steps:

[0027] Step 1: First, perform map preprocessing, obtain a known global elevation grid map, calculate the grid slope s, and read the grid height value h; according to the kinematic characteristics of the hexapod robot, set the maximum terrain slope threshold of the hexapod robot to s t and the terrain height threshold is h t And classify the grids larger than the threshold as infeasible areas; visualize feasible and infeasible areas through color mapping; divide the global map space into several areas Ω i (i=1,2,…,n, n is the total number of regions), through the slope S of each region i , obstacle density D i and terrain roughness R i To calculate the terrain complexity of each area As shown in formula (1):

[0028] I is used to represent the robot's sensitivity to terrain indicators, ranging from [0,1], and α S =I S / (I S +I D +I R ), α D =I D / (I S +I D +I R ), α R =I R / (I S +I D +I R ).

[0029] Step 2: Use the human-computer interaction interface to mark the starting position of the path planning on the map (x start ,y start ) and target position (x goal ,y goal );Get a series of path guidance points input by the driver through the human-computer interaction interface, and use P index Indicates that P index ={(x1,y1),(x2,y2),…,(x n ,y n )};

[0030] The control points of the Bezier curve can be determined based on the obtained n guide points. The number of times the Bezier curve is determined is n-1, and the control points are P0, P1, ..., P n-1 , where P τ =(x τ+1 ,y τ+1 ), τ=0,1,…,n-1, the expression of Bezier curve is shown in (2) where t∈[0,1], As shown in formula (3), by calculating B(t), the smooth path P can be fitted human ;

[0031] The fitted path is judged. If the path passes through an obstacle, the path needs to be refitted.

[0032] Step 3: Cut the map according to the driver's guidance path: The principle diagram of the map cutting algorithm is as follows: Figure 2 As shown, for path P human Each grid point (x j ,y j ), which is located in the region Ω i When taking w in the direction perpendicular to the path i / 2, the area covered is part of the search map. Specifically: First calculate the distance in the area Ω i The terrain complexity is Determine the region Ω by using a width adaptive algorithm i Adaptive width w i , as shown in formula (4), where T max is the upper limit of the terrain complexity of the pre-set area, and w0 is the basic width of the pre-set segmented map;

[0033] Calculate the unit vector perpendicular to the path direction as If the slope of the tangent line of the path at a point is k, then As shown in formula (5):

[0034] Then the boundary point of the search map corresponding to this point is shown in formula (6): Finally, through P human A series of search map boundary points determined by the points on the map determine the search map range.

[0035] Step 4: If Figure 3 As shown in the figure, cost functions for quantifying path length, energy consumption, and path risk are designed respectively. Based on the above cost functions, the actual cost function and expected cost function of the A* algorithm are improved in a targeted manner. Finally, the planned path is smoothed using a cubic spline curve. Specifically,

[0036] Regarding the design of the cost function: the path length cost considers the total length of the path, as directly calculated as the accumulation of all node distances in formula (7): where d k is the distance of the kth path,

[0037] The energy cost considers the energy consumption of the robot under different slope and terrain conditions, taking into account dynamics and resistance, as shown in formula (8): Where m is the mass of the robot, g is the acceleration due to gravity, and θ k =arctan((h k -h k-1 ) / d k ) is the slope h of the kth path k and h k-1 are the grid height values ​​of the kth and k-1th nodes, f is the dynamic efficiency factor, μ friction is the friction coefficient between the robot and the ground;

[0038] The path risk cost is measured by the terrain inclination, terrain complexity, and the deviation between the planned path and the driver's guided path during the robot's driving process. It is believed that in complex terrain, the higher the complexity of the terrain, the closer the planned path is to the driver's guided path, and the lower the risk. The calculation formula is shown in formula (9): is the terrain complexity of region i, d deviation,k is the offset distance between the kth path and the driver's guidance path, d MAX 1 / 2 map width w of region i i / 2,θ k is the current terrain slope, θ MAX is the maximum terrain slope s that the robot can pass t , r and q are nonlinear factors, both greater than 1;

[0039] The offset distance d deviation,k It can be calculated by the shortest distance between the actual path point of the robot and the grid point of the guide path, as shown in formula (10), where P human is the grid point set that the driver’s guidance path passes through, p robot,k is the coordinate of the robot on the kth path;

[0040] The comprehensive cost formula is shown in formula (11), ω E 、ω D 、ω S is the weight factor of each cost, which is used to adjust the impact of different costs on the comprehensive cost and is determined by fuzzy rules; C=ω E ·E+ω D ·D+ω S ·S (11)

[0041] Improvements to the actual cost function and heuristic function of the A* algorithm: g(n) as shown in formula (12) represents the cumulative cost from the starting point to node n, which is a comprehensive cost of energy consumption, stability and path distance, where n parent is the parent node of the current node, C(n parent ,n) is the cost from the parent node to the current node n; g(n)=g(n parent )+C(n parent ,n) (12)

[0042] h(n) (13) is a heuristic function that represents the expected cost from the current node to the target location: Expected energy consumption cost Expected distance cost Expected stabilization cost As shown in formulas (14), (15), and (16) respectively: in is the straight-line distance from the current node to the target node, θ n The slope from the current node to the next node is used as the expected slope of the target path.

[0043] Set fuzzy rules and design three fuzzy input variables according to different task requirements:

[0044] Energy consumption priority requirement T E : Indicates the priority of tasks for energy consumption, ranging from [0,1], divided into the following fuzzy sets {L,M,H}; path length priority T D : Indicates the sensitivity of the task to the path length, ranging from [0,1], and is divided into the following fuzzy sets {L,M,H}; path risk requirement T S : Indicates the importance of the task to the path risk, ranging from [0,1], divided into the following fuzzy sets {L,M,H}; the three input variables are ω E 、ω D 、ω S , is divided into the following fuzzy sets {L,M,H}, and satisfies ω E +ω D +ω S =1;

[0045] The membership functions of input and output are determined as follows:

[0046] Where X is a fuzzy language variable, construct a fuzzy rule table such as Figure 4 As shown, the weighted average method is then used to defuzzify, as shown in formula (20), where ω X is the weight factor (ω E ,ω D ,ω S ), μ i is the membership degree of rule i, v i is the central value of the fuzzy set corresponding to rule i.

[0047] Plan the path using the improved A* algorithm;

[0048] Finally, a cubic spline curve is used for smoothing to obtain a smooth path that meets the task requirements.

Claims

1. An improved A* path planning method based on driver guidance and dynamic map segmentation includes the following steps: Step 1: Preprocessing of the elevation grid map. First, according to the robot's motion performance limit, the elevation grid map is divided into feasible areas and infeasible areas through color mapping to achieve visualization effect; the map is divided into several small areas Ω i , a terrain complexity calculation method is proposed, through the slope R of each area i , obstacle density D i and terrain roughness R i Calculate the terrain complexity of each area Step 2: Through the human-computer interaction interface, the starting position and target position of the path planning are marked in the map, and a series of path guidance points are input based on the driver's intuitive judgment, and the driver's guidance path P is fitted by the Bezier curve. human ; Step 3: Design a map cutting method, combine the driver guidance path obtained in step 2, divide the map in the vertical direction based on the path as the baseline, and design an adaptive map width adjustment algorithm based on the terrain complexity to generate a segmented map; Step 4: Design quantitative evaluation methods for path length, energy consumption, and path risk cost respectively, and make targeted improvements to the actual cost function and heuristic function of the A* algorithm based on the above methods. Finally, use a cubic spline curve to smooth the planned path to obtain a smooth path that meets the task requirements.

2. The elevation grid map preprocessing according to step 1 of claim 1, characterized in that: The maximum slope threshold s of the grid terrain that the hexapod robot can pass is set based on its motion performance limit. t and the terrain height threshold h t , classify the grids larger than the threshold as undriveable areas, and visualize the feasible and infeasible areas through color mapping; Divide the global map space into several regions Ω i , a method for calculating terrain complexity is proposed. By calculating S i , D i and R i To calculate the terrain complexity of each area where α S , α D , α R is the weight of each terrain, which is adjusted according to the robot's sensitivity to different terrains. I represents the robot's sensitivity to terrain indicators, ranging from [0,1], and α S =I S / (I S +I D +I R ), α D =I D / (I S +I D +I R ), α R =I R / (I S +I D +I R ).

3. Generate a segmented map according to the map cutting algorithm described in step 3 of claim 1, characterized in that: According to the driver's guidance path P human The map grid determines a series of grid points (x j ,y j ), which is located in the region Ω i When taking w in the direction perpendicular to the path i / 2, the area covered is part of the search map, w i Determined by the width adaptive adjustment algorithm, as shown in formula (1), T max is the upper limit of the terrain complexity of the pre-set area, and w0 is the basic width of the segmented map; Calculate the unit vector perpendicular to the path direction As shown in formula (2): The boundary point of the search map corresponding to this point is determined as shown in formula (3).

4. According to the quantitative evaluation method of path length, energy consumption and path risk cost in step 4 of claim 1 and the targeted improvement of the actual cost function and heuristic function in the A* algorithm, it is characterized by: The path length cost is the total length of the path, which is directly calculated as the accumulation of all node distances, as shown in formula (4): k is the distance from node k-1 to node k; The energy consumption cost is the energy consumption calculation formula of the robot under different slope and terrain conditions as shown in formula (5): Where m is the mass of the robot, g is the acceleration due to gravity, and θ k is the slope of the kth path, f is the dynamic efficiency factor, μ friction is the friction coefficient between the robot and the ground; The path risk cost is measured by the inclination of the terrain, the complexity of the terrain, and the deviation between the planned path and the driver's guided path during the robot's driving process; It is believed that in complex terrain, the higher the complexity of the terrain, the closer the planned path is to the driver's guidance path, and the smaller the risk. The calculation formula is shown in formula (6): is the terrain complexity of region i, d deviation,k is the offset distance between the kth path and the driver's guidance path, d MAX 1 / 2 map width w of region i i / 2,θ k is the current terrain slope, θ MAX is the maximum terrain slope s that the robot can pass t , r and q are nonlinear factors, both greater than 1; The offset distance d deviation,k It can be calculated by the shortest distance between the actual path point of the robot and the grid point of the guide path, as shown in formula (7), where P human is the grid point set that the driver guides through, p robot,k is the coordinate of the robot on the kth path; In summary, the comprehensive cost formula is shown in formula (8): C=ω E ·E+ω D ·D+ω S ·S (8) The improvements of the heuristic function and actual cost function of the A* algorithm are shown in formulas (9) and (10), respectively, where n represents the node n, n parent represents the parent node of node n, goal represents the target node, g(n) represents the cumulative cost from the starting point to node n, the cost integrates energy consumption, path risk and path length, and h(n) represents the expected cost from the current node to the target location; g(n)=g(n parent )+C(n parent ,n) (9) The expected energy cost Expected path length cost Expected risk cost As shown in formula (11), (12), (13), respectively, is the straight-line distance from the current node to the target node; where ω E ,ω D ,ω S It is the weight factor of each cost, which is used to adjust the impact of different costs on the total cost. Its value will be determined by fuzzy rules according to the requirements of different tasks for the path.

Images