CN117451068A

CN117451068A - Hybrid path planning method based on improved A-algorithm and dynamic window method

Info

Publication number: CN117451068A
Application number: CN202311419045.8A
Authority: CN
Inventors: 胡兴柳; 马尹琪; 杨慧珍; 顾海华
Original assignee: Jinling Institute of Technology
Current assignee: Jinling Institute of Technology
Priority date: 2023-10-27
Filing date: 2023-10-27
Publication date: 2024-01-26

Abstract

The invention provides a hybrid path planning method based on an improved A-algorithm and a dynamic window method, and belongs to the technical field of robot path planning. The method combines global prediction and local real-time planning into a mixed two-layer algorithm, namely, an improved A-algorithm is adopted to complete global prediction, so as to obtain a globally optimal path node; and then, selecting inflection points from the nodes as a guide of real-time planning, adopting an improved dynamic window method to dynamically avoid the obstacle in real time by a real-time planning layer, continuously running each local target point, and finally reaching the target point to finish the task. The invention adopts an improved A-algorithm, and can find the shortest path in the search space; meanwhile, the optimization dynamic window method is used for planning the local path, and the window size can be adjusted according to the change of the dynamic environment, so that the safety of the path is ensured, and the practicability and flexibility of the dynamic obstacle avoidance method are improved.

Description

Hybrid path planning method based on improved A-algorithm and dynamic window method

Technical Field

The invention belongs to the technical field of robot path planning, and particularly relates to a hybrid path planning method based on an improved A-algorithm and a dynamic window method.

Background

With the continuous development of robotics, more and more robots are being used to provide various services and even replace the work of people. Autonomous navigation is a key for the robot to realize intelligence and play a role, and path planning is an important component of the robot's automatic navigation capability. The path planning problem of mobile robots is a hot spot research content at present. Path planning refers to determining the optimal path of a robot from a starting position to a target position in a given environment using sensors to acquire information.

Global path planning and local path planning are basic modules for realizing autonomous navigation of a mobile robot. Traditional path planning algorithms perform well in simple environments, but face problems of high complexity and poor real-time performance in complex environments. To solve this problem, researchers have proposed a mixed path planning concept. The hybrid path planning combines global path planning and local path planning, generates a preliminary path through the global path planning, and optimizes and corrects the path in the local path planning so as to meet the real-time performance and the high efficiency in a complex environment. The hybrid path planning algorithm not only can improve the path planning efficiency of the robot, but also can improve the motion capability and adaptability of the robot in a complex environment. Researchers generally apply A-algorithm to global path planning in the past, and on the basis of the obtained global path, a dynamic window method is used for carrying out local path planning and adjustment, and the global planning capacity of the A-algorithm and the local planning capacity of the dynamic window method are utilized to improve the efficiency and quality of the overall path planning.

For global path planning optimization, the A-algorithm construction model is simple, the searching efficiency is high, and the solution obtained for the static scene is close to the optimal path. However, the algorithm a has many disadvantages, such as more inflection points of the generated path and insufficient smoothness of the path. For these problems, many scholars have proposed improvement ideas. For example, a smooth a-x model is established to optimize the path, so that the search efficiency of the traditional a-x algorithm is further improved; the gravity navigation information is added into the heuristic function, so that the algorithm searching efficiency is improved; and introducing a polynomial curve to smooth the path, and optimizing the path quality. In the conventional improvement method, three aspects of path planning efficiency, path length and path rotation angle cannot be considered, so that research on an algorithm A searching strategy, a cost function and a path smoothing algorithm is very important.

For local path planning, the planning efficiency and path quality of the traditional dynamic window method in various scenes need to be further improved, and many researchers improve the traditional dynamic serial port algorithm. For example, by expanding the space of the track evaluation function and the degree evaluation function, the algorithm jumps out of local optimization, and planning capacity and instantaneity are improved; the robot pre-track which is not intersected with the obstacle is selected based on the differential manifold tangential vector, and the obstacle quantity factor and the direction angle change factor are introduced to improve the evaluation function, so that the running safety of the robot in the obstacle dense area is improved.

From the above analysis, there are many areas where hybrid path planning algorithms are worth improving and optimizing. Firstly, aiming at an A-algorithm of global path planning, not only the length of an optimal path is considered, but also the searching efficiency of the path planning is considered, how to improve the searching strategy of the A-algorithm, and then the duty ratio of a heuristic function and an evaluation function in a cost function is adjusted, so that the problem to be solved is solved. In addition, for the problem that the traditional dynamic window method can be in local optimum for more obstacles, a new evaluation function needs to be designed to ensure that the robot moves safely and jumps out of the local optimum.

Disclosure of Invention

Aiming at the defects in the prior art, the invention provides a hybrid path planning method based on an improved A-based algorithm and a dynamic window method.

In order to achieve the above purpose, the present invention adopts the following technical scheme:

a hybrid path planning method based on an improved a algorithm and a dynamic window method, comprising:

performing environment measurement through a sensor, constructing a grid environment model matched with the environment characteristics, and determining the positions of a starting point and a target point in the grid environment model;

based on the constructed grid environment model, adopting an A-algorithm to complete global prediction to obtain a globally optimal path node;

Selecting an inflection point from the globally optimal path nodes as a local target point for real-time planning, and adopting a dynamic window method to carry out local path planning for avoiding the obstacle;

and continuously traversing each local target point according to the route planned by the local path, and finally reaching the target point.

In order to optimize the technical scheme, the specific measures adopted further comprise:

further, the method for completing global prediction by adopting an a-x algorithm to obtain a globally optimal path node comprises the following steps:

s1.1: creating path node sets open_list and close_list, wherein the open_list stores path nodes to be calculated by the robot, and the close_list stores nodes for which the cost has been calculated by the robot;

s1.2: confirming that the open_list is empty, storing the starting point and 8 neighborhood nodes nearby the starting point into the open_list, and setting the starting point as a father node of the 8 neighborhood nodes; then storing the starting point in close_list and deleting the starting point from open_list;

s1.3: traversing the nodes in the open_list, calculating the node with the minimum cost value, moving the node out of the open_list, adding the close_list, and enabling the node to be a node on the optimal path from the starting point to the target point;

s1.4: setting the node on the last determined optimal path as a father node, storing N neighborhood nodes around the father node in open_list, wherein N is less than or equal to 8, calculating the node with the minimum cost value in the N neighborhood nodes, moving the node out of the open_list, adding close_list, and then setting the node as a node on the optimal path from the starting point to the target point;

S1.5: the step S1.4 is repeated until the target point is detected, ending the iteration, where the node stored in close_list is the globally optimal path node from the start point to the target point.

Further, in steps S1.3 and S1.4, the cost value is calculated by a cost function as follows:

f(n)＝g(n)+w(n)×h(n)

where f (n) represents a cost function from a start point to a node n; g (n) represents the actual cost from the starting point to node n; h (n) represents the estimated cost from node n to the target point, h (n) =max (|x) _n -x _g |，|y _n -y _g |)，(x _n ，y _n ) Representing the coordinates of node n, (x) _g ，y _g ) Representing coordinates of a target point; w (n) represents the observation probability of the node n, and is calculated as follows:

a rectangular area taking a target point as a center is planned, and the number Nx of nodes which need to pass through in the rectangular area is calculated, wherein Nx and Ny are respectively the difference values of a node n and the target point in the horizontal and vertical directions; traversing the positions of all the obstacles, and calculating the number node_obs of the obstacles in the rectangular area; calculating the observation probability of node n

Further, in step S1.4, the depositing N neighborhood nodes around the parent node in the open_list specifically includes:

8 neighborhood nodes around the father node are selected, the 8 neighborhood nodes are traversed in sequence, whether the path from the father node to the neighborhood node is blocked by the obstacle is judged according to the neighborhood where the obstacle is located, if yes, the neighborhood node is deleted, and if not, the neighborhood node is added into the open_list.

Further, after the globally optimal path node is obtained, the following processing is performed to delete the redundant path node:

initializing a forward search from a starting point and a reverse search from a target point, and setting termination conditions of the forward search and the reverse search;

and simultaneously performing forward search and reverse search, and judging the search direction in each step: if the forward search is closer to the target point, continuing the forward search; if the reverse search is closer to the starting point, continuing the reverse search;

in each step, it is determined whether the forward search and the reverse search meet: if the forward search and the reverse search meet, the search is terminated, and an optimal path is generated by recording paths of the forward search and the reverse search;

and carrying out further smoothing processing on the generated optimal path.

Further, the local path planning for avoiding the obstacle by adopting the dynamic window method comprises the following steps:

s2.1: initializing the state of the robot, including position, speed and acceleration;

s2.2: generating a plurality of speed and acceleration combinations according to a dynamics model of the robot;

s2.3: predicting the track of the robot in a short time according to a dynamics model of the robot for each speed and acceleration combination;

S2.4: according to the environment map or the sensor data, evaluating whether each track collides with an obstacle or exceeds a map boundary;

s2.5: calculating an evaluation index of each track according to the evaluation result of the predicted track;

s2.6: selecting the optimal track as the next action of the robot according to the evaluation index;

s2.7: executing the selected action, and updating the state of the robot;

s2.8: steps S2.1-S2.7 are repeated until the target point is reached or no further movement is possible.

Further, in step S2.5, the evaluation index of each track is calculated by the following evaluation function:

G(v，ω)＝α×head(v，ω)+γ×path(v，ω)+σ×occ(v，ω)+β×goal(v，ω)

wherein G (v, ω) represents an evaluation function that calculates an evaluation index of the trajectory; head (v, ω) represents the robot bearing evaluation sub-function; path (v, ω) represents a path evaluation sub-function; occ (v, ω) represents a collision evaluation sub-function; gol (v, ω) represents a robot local path end-to-target point distance evaluation sub-function; sigma, alpha, beta and gamma respectively represent coefficients of each evaluation sub-function; v is the linear velocity, ω is the angular velocity;

wherein, the distance between the robot local path end and the target pointThe evaluation sub-function gold (v, ω) is determined by: calculating an overall straight-line distance L between the predicted trajectory end and the target point using Euclidean distance ₁ Predicting the path length L between the end of the trajectory and the target point using Manhattan distance calculation considerations that can only be moved along the grid ₂ Predicting a straight-line distance L between the trajectory end and the target point taking into account the diagonal movement using the diagonal distance calculation ₃ Through L ₁ 、L ₂ 、L ₃ And adding and adjusting the weights of the local path ends to obtain the distance between the local path ends and the target point as an output result of the gold (v, omega).

Further, the evaluation index of each track is normalized to the same magnitude by head (v, ω), path (v, ω), occ (v, ω), gold (v, ω) before calculation.

The present invention also proposes a computer-readable storage medium storing a computer program, wherein the computer program causes a computer to execute the hybrid path planning method based on the modified a-algorithm and the dynamic window method as described above.

In addition, the invention also provides electronic equipment, which is characterized by comprising: the system comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor executes the computer program to realize the hybrid path planning method based on the improved A-type algorithm and the dynamic window method.

The beneficial effects of the invention are as follows:

1. the invention adopts an improved A-algorithm to plan the global path planning, optimizes the cost function, the heuristic function, the search rule and the like, and can quickly find the shortest path in the search space. And then, the improved dynamic window method is used for carrying out local path planning, evaluation functions and the like are optimized, and the window size can be adjusted according to the change of dynamic environment, so that the safety of a path is ensured, and the practicability and the flexibility of the dynamic obstacle avoidance method are improved. The improved A-phase algorithm adopted by the invention selects the optimal path as early as possible, and meanwhile, the path safety is ensured by an optimized dynamic window method, so that the final path planning is more fit with the optimal solution, and random obstacle avoidance and dynamic obstacle avoidance can be realized on the premise of being close to the global path;

2. the diagonal distance is introduced into the A-algorithm, and the distance between two points can be estimated more accurately, so that the path planning precision is improved, the track optimization is performed, the path length and the turning times are reduced, the path planning searching efficiency is higher, meanwhile, the robot kinematic model is met, the robot is prevented from colliding with the environment, and a better node sequence is provided for a subsequent dynamic window method; meanwhile, an evaluation function of a dynamic window method is improved, so that the evaluation function can be better combined with an A-algorithm, dynamic obstacle avoidance of an optimal solution is realized, and an effective solution is provided for autonomous navigation and obstacle avoidance in the fields of robots, automatic vehicles and the like.

Drawings

Fig. 1 is a conventional a-algorithm global path planning flowchart.

Fig. 2a is a schematic diagram of a global path planning simulation of a conventional a-algorithm.

Fig. 2b is a schematic diagram of a modified a-algorithm global path planning simulation.

Fig. 3 is a flow chart of a conventional dynamic windowed local path planning method.

Fig. 4a is a schematic diagram of a conventional dynamic windowed local path planning simulation.

Fig. 4b is a schematic diagram of a modified dynamic windowed partial path planning simulation.

Fig. 5 is a flow chart of a hybrid algorithm implementation technique.

Fig. 6a is a schematic diagram of a conventional a-algorithm and conventional dynamic window method hybrid algorithm path planning simulation.

Fig. 6b is a schematic diagram of a modified a-algorithm and modified dynamic window method hybrid algorithm path planning simulation.

Detailed Description

The invention will now be described in further detail with reference to the accompanying drawings.

In an embodiment, the invention provides a hybrid path planning method based on an improved A-algorithm and a dynamic window method, which combines global prediction and local real-time planning into a hybrid two-layer algorithm, namely, adopting the improved A-algorithm to complete global prediction and obtain a globally optimal path node; then, inflection points are selected from the nodes to serve as guidance of local real-time planning, a local real-time planning layer dynamically avoids obstacles in real time by adopting an improved dynamic window method, each local target point is continuously operated, and finally, the target point is reached to complete a task;

Firstly, an improved A-algorithm prediction process is used in the environment to obtain a global optimal path, inflection points of the path are extracted to serve as local target points of local path planning, and an improved dynamic window method is adopted to conduct real-time planning processing, so that the overall better performance of the path is guaranteed by the mixed two-layer algorithm, and the planning task can be responded in real time in the dynamically-changed environment.

1. Improvement of global path planning algorithm

The algorithm a adopts heuristic search, namely a heuristic function is added on the basis of the Dijkstra algorithm. The main principle is as follows: the distance from the current point to the target point is estimated by a heuristic function, this distance being an estimated value, not necessarily an exact value. Each time the node closest to the starting point is selected as the current node, the distance h (n) from the node to the target point is then calculated and added to the distance g (n) from the starting point to the node, resulting in a cost function value. The algorithm always prioritizes the node with the smallest cost function value because it is more likely to be the optimal path than the other paths, the flow chart is shown in fig. 1.

The algorithm comprises the following specific steps:

let the starting point be A and the target point be F.

1) The set of path points open_list and close_list are created. Wherein open_list stores the path nodes to be calculated by the robot, and close_list stores the nodes for which the robot has calculated the cost.

2) It is checked whether open_list is empty. Storing a starting point A and 8 neighborhood points near the starting point A in an open_list, and setting a node A as a father node of the neighborhood nodes; the starting point is then stored in close_list and node a is deleted from open_list.

3) And traversing the path points in the set open_list, calculating a point S with the minimum cost value F (n) in the set, moving out of the open_list, adding the close_list, and forming the point as a node on the optimal path from the starting point A to the target point F.

4) The node S is set as a father point, 8 neighborhood nodes around the node S are stored in an open_list, a point with the minimum cost value F (n) in the 8 nodes is calculated, the open_list is removed, a close_list is added, and the point becomes a node on the optimal path from the starting point A to the target point F.

5) And repeatedly executing the step 4) until the target point F is detected, and ending the iteration. The node stored in the set close_list is the optimal path from the starting point to the target point.

However, due to the limitation of the heuristic function, the traditional A-type algorithm can cause different calculation methods to appear when the algorithm calculates the global cost value, thereby influencing the final planned path distance; when the robot moves, the distance between the robot and the target point can be changed, the proportion between g (n) and h (n) can be changed through the weighting coefficient, and the path searching efficiency is increased so as to realize faster and shortest path planning. The traditional a-algorithm is improved by the following heuristic functions, weight coefficient increasing, searching neighborhood optimizing and path smoothing optimizing.

(1) Heuristic functions. According to the evaluation function of the traditional A-algorithm, g (n) is known to control the searching breadth of the algorithm only, h (n) is a heuristic function, and the heuristic function mainly provides the correct expansion direction for the robot path planning algorithm, so that the influence of the selection of the heuristic function on the algorithm is great. There are three main heuristic functions:

formula 1: h (x) = |x _n -x _g |+|y _n -y _g |

Formula 2:

formula 3: h (n) =max (|x) _n -x _g |，|y _n -y _g |)

In (x) _n ，y _n ) Representing the coordinates of node n;(x _g ，y _g ) Representing the coordinates of the target point.

Equation 1 is a manhattan distance, which calculates a distance between a start point and a target point by adding distances of two points in horizontal and vertical directions, but does not consider a path of oblique lines, resulting in a low calculation amount, high efficiency, and easy missing of an optimal path; equation 2 is the euclidean distance, also called the straight line distance, the distance between the starting point and the target point is determined by the straight line distance between the calculated quantities, but when facing the grid map, a large number of floating point numbers appear in the calculation, which results in low algorithm searching efficiency, increased calculated quantity and error rate; equation 3 is a diagonal distance, which can be used to represent the distance between two n-dimensional vectors, can be used for comparing unordered data, has the advantages of symmetry, no influence of scale and rotation, suitability for multiple data, and the like, and therefore selects the diagonal distance as a heuristic function for improving the a-algorithm.

(2) The weight coefficient is increased. The evaluation function of the algorithm a is determined by g (n) and h (n), but the heuristic function h (n) plays a decisive role, and the proportional relationship between h (n) and g (n) also affects the algorithm search speed and breadth.

1) If h (n) is zero, only g (n) acts, belonging to a single search, where the a-algorithm is degenerated to Dijkstra algorithm.

2) If h (n) is smaller than the actual cost, the algorithm can plan an optimal path, but the smaller h (n) is, the more nodes are traversed, the search space is increased, and the algorithm efficiency is reduced.

3) If h (n) is equal to the actual cost, the algorithm is in an ideal state, the path planning speed is high, and an optimal path can be planned.

4) If h (n) is greater than the actual cost, the algorithm searches fast, but the planned path is not necessarily optimal.

5) If h (n) is much greater than g (n), it means that only h (n) acts with g (n) being negligible, and the a algorithm becomes breadth-first.

Therefore, a weight coefficient omega (n) can be added to h (n), and the size of omega (n) can be dynamically adjusted by judging the relation between h (n) and the actual cost, so that the proportional relation between h (n) and g (n) can be changed. When searching starts, it is more important to quickly reach the area where the destination is located; at the end of the search, it is more important to get the best path to the target. When the distance from the target point is far, h (n) should be large, ω (n) should also be large, and the algorithm a will expand to the target point as soon as possible; when h (n) should be smaller as closer to the target point, ω (n) should also be smaller, at which time the a algorithm will tend to search for the optimal path and slow down the search.

f(n)＝g(n)+w(n)×h(n)

Where f (n) represents the cost function from the start point to node n, g (n) represents the dissipation function, i.e. the actual cost from the start point to node n, and h (n) represents the heuristic function, i.e. the estimated cost from node n to the target point. ω (n) represents the observation probability of the node and reflects the environmental conditions around the node.

The observation probability of the node is calculated by the obs_array function, the position of the node is defined as (node_x, node_y), and the position of the Target point is defined as (target_x, target_y). Meanwhile, the position coordinates of a set of obstacles, i.e. a CLOSED matrix, are known, where Nobs is expressed as the number of obstacles.

First, a rectangular area with a width of 2×0.8=1.6 centered on the target point is planned, and coordinates of the lower left corner and the upper right corner thereof are (x_min, y_min) and (x_max, y_max), respectively. Then, the number Nx of nodes that need to pass through in this rectangular area is calculated. Nx and Ny are differences between the current node and the target point in the horizontal and vertical directions.

Then, the positions of all obstacles are traversed, and if the position of a certain obstacle is within this rectangular area, the obstacle counter node_obs within the travel process area is incremented by one. Finally, the observation probability of the node is set to be the number of all obstacles node_obs in this rectangular area divided by nx×ny, i.e

In path planning, the observation probability can be used to evaluate the feasibility of a node, i.e. whether the node can reach the target point. The greater the probability of observation around a node, the more obstacles around the node, the less likely a path will pass through the node. Therefore, the observation probability of the node can be used to adjust the priority of the node in the search algorithm, so that the search algorithm can find a feasible path more quickly.

(3) The search rules are optimized. The traditional A-algorithm generally sets 8 neighborhoods around the father node as search neighborhoods, and the method ensures the safety of path planning to a certain extent, reduces the probability of collision with obstacles, but also slows down the speed of path search, so that the search speed of the traditional A-algorithm can be improved by optimizing the search rule.

The conventional a-algorithm search neighborhood is shown in the following table:

table 1 search neighborhood of conventional a-x algorithm

Neighborhood 1	Neighborhood 2	Neighborhood 3
			Neighborhood 4	Father node	Neighborhood 5
Neighborhood 6	Neighborhood 7	Neighborhood 8

In general, the robot calculates the cost of the neighborhood 1-8 no matter where the obstacle is, and provides a search neighborhood selection rule according to different directions of the obstacle: (since the map is a grid map in a square form, a collision occurs from the center point of one point to the center point of another point, and no diagonal line can be drawn when the map hits an obstacle)

1) When the obstacle is located in the neighborhood 2, then the neighborhood 1, the neighborhood 2 and the neighborhood 3 are deleted in the open_list because the paths from the parent start point to the neighborhood 1, 2, 3 are blocked by the obstacle and the target point cannot be reached through the obstacle. Therefore, only 4-8 cost values of the neighborhood are calculated, and 3 calculation amounts are reduced.

2) When the obstacle is located in the neighborhood 4, the neighborhood 1, the neighborhood 4 and the neighborhood 6 are deleted from the open_list, and paths from the parent node to the neighborhood 1, the neighborhood 4 and the neighborhood 6 are blocked by the obstacle and cannot reach the target point through the obstacle in the same principle. Therefore, only the cost values of the neighborhoods 2, 3, 5, 7 and 8 are calculated, and 3 calculation amounts are reduced.

3) When the node is located in neighborhood 3, no processing is done since no more detailed information is available.

Note that: neighborhood 7, reference 1); neighborhood 5, reference 2); neighborhood 1, neighborhood 6, neighborhood 8 reference 3).

(4) Path smoothness is optimized. Because the map is constructed by using the grid method, when the conventional A-algorithm performs path planning, the operation center point of the path is positioned at the center of the grid, so that more turns can be necessarily caused in the path, and the problems of longer path distance, poor smoothness and the like occur. For the above problems, a bidirectional search method is adopted to optimize the smoothness of the path. Aiming at the distance between two non-adjacent nodes in the path nodes, if the distance can be smaller than the planned distance between the two nodes and no obstacle on the path can collide, redundant path points can be deleted.

The path smoothing steps are as follows:

1) Defining a starting point and a target point: and determining a starting point and a target point of the path planning.

2) Initializing forward and reverse searches: forward and reverse searches are performed starting from the start point and the target point, respectively.

3) Setting a search termination condition: the termination conditions for the forward and reverse searches are set. It may be to find the meeting point or search for reaching the boundary.

4) Forward and reverse searches are performed simultaneously: in each step, both forward and reverse searches are performed simultaneously. And judging the searching direction according to the progress conditions of the current forward searching and the reverse searching. If the forward search is closer to the target point, continuing the forward search; if the reverse search is closer to the starting point, the reverse search is continued.

5) Judging the meeting point: in each step, it is determined whether the forward search and the reverse search meet. Whether or not to meet may be determined by checking whether or not the currently searched node has been accessed in a search in the other direction.

6) Generating an optimal path: if the forward search and the reverse search meet, this indicates that an optimal path is found. The optimal path may be generated by recording the paths of the forward search and the reverse search.

7) Smooth path: and carrying out path smoothing processing on the generated optimal path. Curve fitting, interpolation, etc. may be used to smooth the path, making it smoother.

8) Returning to the optimal path: and returning the optimal path after the path smoothing treatment as a result.

In order to verify the performance of the improved a-algorithm, a simulation experiment is set on a grid map of 30×30, and the comparison analysis is performed on the conventional a-algorithm and the improved a-algorithm from 4 indexes of planned path length, traversal area, path-finding time and inflection point number, respectively. Simulation results as shown in fig. 2a and 2b, black squares are obstacles, grey is the traversal area, and solid lines are the planned paths. The simulation data are shown in table 2.

Table 2 simulation comparison of conventional a-algorithm and modified a-algorithm

Algorithm name	Path length/m	Traversing region/number	Road-finding time/s	Number of inflection points/number of inflection points
					Traditional a-algorithm	36.87	239	0.15	10
Improved a algorithm	36.56	119	0.07	3

Under the condition of the starting point, the number of inflection points of the traditional A-algorithm is 10, the path length is 36.87 meters, the traversing area is 239, and the path searching time is 0.15 seconds. The improvement of the algorithm A is remarkable in improvement, and the number of inflection points, the path-finding time, the traversing area and the path length are respectively improved by 70%, 3.33%, 50.21% and 0.82% compared with the former. The improved A-algorithm solves the problems of more turning times, longer search time, longer planned path and the like in the path planning process of the traditional A-algorithm.

2. Improvement of local path planning algorithm

After the global path planning is completed, the robot knows where to go, but when the robot encounters an emergency such as an obstacle, an unknown area and the like in the driving process, the robot needs to correct the track through the local path planning so as to realize the obstacle avoidance function. The main operation mode of the dynamic window method is as follows: a plurality of sets of speeds are sampled in a speed (v, ω) space, v being the linear speed, ω being the angular speed, and the trajectory of the robot over a certain time period at these speeds is simulated. And after a plurality of groups of tracks are obtained, evaluating the tracks, and selecting the speed corresponding to the optimal track to drive the robot to move.

As shown in fig. 3, a flow chart of the dynamic window method is shown, and the specific steps are as follows:

1) The state of the robot or vehicle is initialized, including position, speed, acceleration, etc.

2) From a kinetic model of the robot or vehicle, a set of possible velocity and acceleration combinations, called a motion space, is generated.

3) For each combination of speed and acceleration in the motion space, the trajectory of the robot or vehicle in a short time is predicted from the dynamics model of the robot or vehicle.

4) Based on the environmental map or sensor data, the safety and feasibility of each track is assessed, for example, whether it collides with an obstacle or exceeds the map boundary.

5) According to the safety and feasibility of the predicted trajectories, evaluation indexes of each trajectory, such as distance from the target, distance from the obstacle, speed, acceleration, and the like, are calculated.

6) According to the evaluation index, the optimal track is selected as the next action of the robot or the vehicle.

7) And executing the selected action and updating the state of the robot or the vehicle.

8) Repeating steps 2) to 7) until the target point is reached or the movement cannot be continued.

When the traditional dynamic window algorithm evaluates the path, due to different speed solution units and magnitudes, a certain numerical value is large, so that the favorable situation is occupied, and the path optimization result is influenced. The local path algorithm is lack of guidance of the global path, and when facing more obstacles on the way to the next local target point, the local path algorithm is actively far away and falls into local optimum, so that the global path length is prolonged, and the efficiency is poor. The DWA algorithm can be improved both in terms of the evaluation function and in terms of the normalization process.

(1) And optimizing the evaluation function. After the speed acquisition is completed, each track is scored by the dynamic window method through an evaluation function, and the evaluation indexes of the traditional dynamic window method mainly comprise: the robot direction, speed and distance from the obstacle ensure the safety of the robot movement to a certain extent, but are easy to sink into local optimum, and a new evaluation index gold (v, omega) and weights sigma, alpha, beta and gamma are added, so that the new evaluation function is as follows:

G(v，ω)＝α×head(v，ω)+γ×path(v，ω)+σ×occ(v，ω)+β×goal(v，ω)

Wherein head (v, ω) represents a robot bearing evaluation sub-function; path (v, ω) represents a path evaluation sub-function; occ (v, ω) represents a collision evaluation sub-function; gol (v, ω) represents a robot local path end-to-target point distance evaluation sub-function, a local path end being a local target point; sigma, alpha, beta, gamma denote coefficients of the respective evaluation sub-functions.

And (3) a head (v, omega) function, namely converting the orientation angle of the current position of the robot into an angle system. And then calculating the azimuth angle between the target point and the current position of the robot. Next, a difference between the target azimuth and the robot orientation angle is calculated from a magnitude relation between the target azimuth and the current orientation angle of the robot. Subtracting the robot orientation angle from the target azimuth if the target azimuth is greater than the robot orientation angle; otherwise, the target azimuth is subtracted from the robot heading angle. Finally, the code subtracts the difference between the target azimuth angle and the robot orientation angle through 180 to obtain an evaluation result head of the robot orientation target point. The evaluation result has a value ranging from 0 to 180, and a larger value indicates a better degree of robot orientation to the target point.

path (v, ω) function, accepts three parameters (given position, obstacle position, radius of obstacle). The distance is first initialized to a large value. It then traverses all the obstacles, calculating the distance of each obstacle from the given location. If the calculated distance is smaller than the previous distance, it will be updated to the minimum distance. Finally, the function returns the minimum distance path.

The gol (v, ω) function first simulates the overall straight line distance between the end of the predicted trajectory and the target point using euclidean distance computation dynamic windowing. The path length between the end and the target point is then calculated using the manhattan distance, taking into account the fact that only movement along the grid is possible. Finally, the straight line distance between the end and the target point is calculated using the diagonal line distance, taking into account the case of diagonal line movement. The specific calculation method is to sum the square of the x coordinate difference value and the square of the y coordinate difference value of the end coordinates, and then take the square root. By adjusting the weights of different distances, the distance between the tail end of the partial path and the target point is better estimated and used as the output result of the function.

(2) Normalization processing, which is not simple numerical comparison, needs to compare each velocity solution after calculating multiple groups of velocity solutions, but rather normalizes (smoothes) each numerical value, and converts different evaluation indexes to the same magnitude so as to perform comprehensive evaluation and comparison to select an optimal velocity solution. The expression is as follows:

wherein: n represents all the trajectories employed; i represents the current trajectory to be evaluated. head (i) has the same meaning as head (v, ω) previously described, i.e., (v, ω) on track i.

In order to verify the performance of the improved dynamic window algorithm, a simulation experiment is set on a grid map of 20 multiplied by 15, and the traditional dynamic window algorithm and the improved dynamic window algorithm are respectively compared and analyzed from 3 indexes of planned path length, path finding time and turning times. If a robot collides with an obstacle during the path planning, a path planning failure is declared. As shown in fig. 4a and 4b, the simulation results show that the light gray squares (13, 2) at both ends of the path are used as starting points, the dark squares (2, 20) are used as target points, the other black squares are used as obstacles, and the solid line is the path planned by the algorithm. The simulation data are shown in table 3.

Table 3 simulation contrast of traditional dynamic Window algorithm and improved dynamic Window algorithm

Algorithm name	Path length/m	Road-finding time/s	Number of turns/times
				Traditional dynamic window algorithm	24.83	19.97	5
Improved dynamic window algorithm	19.47	17.88	3

It can be seen that under the condition of the same map and starting point, the traditional dynamic window algorithm turns 5 times, the path length is 24.83 m, and the path-finding time is 19.97 seconds. The improved dynamic window algorithm is obviously improved in the aspects of path length, path finding time and turning times, and compared with the former, the improved dynamic window algorithm is respectively improved by 21.59%, 10.47% and 40%, and the improved dynamic window algorithm is obviously improved in all aspects.

3. Hybrid algorithm analysis

By adopting a two-layer path planning strategy, the advantages of the two algorithms are fully combined, so that not only can dynamic obstacles be effectively avoided in real time, but also the characteristic of global path optimization can be ensured. Meanwhile, the improved dynamic window method fills the blank that the processing capacity of the algorithm A to unknown obstacles is insufficient through various mutually complementary defects, manages the local target points obtained by the improved algorithm A, and ensures the optimal path. Meanwhile, the local target management can also avoid the occurrence of the problem of the minimum point.

As shown in fig. 5, the fusion process of the modified a-algorithm and the modified dynamic window method is specifically as follows:

1) The environment is measured by the relevant sensors, and a real map is constructed according to the characteristics of the environment and the characteristics of the relevant objects, so that a grid environment model matched with the characteristics of the environment is obtained.

2) The improved global path planning algorithm is a stage from global path planning to local path planning.

3) And finding out corresponding inflection points from the predetermined path nodes, and taking the inflection points as reference points of real-time planning.

4) And performing secondary operation on the real-time planning layer by adopting an improved dynamic window method, and performing real-time processing by taking inflection points on the global path as sub-points so as to avoid obstacles.

5) Under the guidance of a real-time planning algorithm, each local target point is continuously traversed, and finally the target point is reached.

In order to analyze and compare the difference between the improved mixed path planning algorithm and the traditional algorithm, a simulation experiment is carried out on the mixed path planning algorithm. Wherein: the starting point coordinates are (1, 17); the coordinates of the target point are (20, 2); black is an obstacle; light grey is a moving obstacle; grey is a fixed obstacle; the solid line is a path planning curve; failure of path planning is declared if a robot collides with an obstacle during path planning. Simulation results are shown in fig. 6a and 6b, and simulation data are shown in table 4.

The difference between the two is evident from fig. 6a and 6 b. The difference between them is very evident in the initial phase, compared to the two figures. The improved hybrid path planning algorithm enables the robot to walk along the global path all the time, avoids redundant turning, and shortens the path length. When a turn is encountered, the former tends to keep the robot as far away from the obstacle as possible, resulting in a deviation from the global path and appearing somewhat jerky when the gray obstacle is subsequently avoided. On the contrary, the latter keeps fit with the global track as much as possible on the basis of ensuring that the robot does not collide, so that the following avoidance of grey obstacles is smoother. After avoiding the grey obstacle, the former runs straight on the target point, and the distance is probably shortest, but the robot is put in a strange environment due to the deviation from the global path, so that the danger is high. And the robot is returned to the global path at the fastest speed after the avoidance is completed, so that the safety of the robot is ensured. As can be seen from table 4, the improved hybrid path planning algorithm is improved by 7.36% and 9.46% in path length and path-finding time compared with the conventional path planning algorithm.

Table 4 improved hybrid path planning algorithm and traditional path planning algorithm result comparison

Algorithm name	Path length/m	Road-finding time/s	Number of turns/times
				Traditional path planning algorithm	26.91	74.21	3
Improved hybrid path planning algorithm	24.93	67.19	3

The invention integrates the improved A-algorithm and the improved dynamic window method, and effectively solves the problems of low searching efficiency, unsmooth path, more redundant nodes and incapability of randomly avoiding barriers and dynamically avoiding barriers in a complex environment of the traditional A-algorithm. And the search efficiency is higher, and the path length and the smoothness are optimized.

In another embodiment, the present invention further provides a computer readable storage medium storing a computer program, where the computer program causes a computer to execute the hybrid path planning method based on the modified a-x algorithm and the dynamic window method according to the first embodiment.

In another embodiment, the present invention further provides an electronic device, including: the hybrid path planning method based on the modified a-algorithm and the dynamic window method according to the first embodiment is implemented when the processor executes the computer program.

In the embodiments disclosed herein, a computer storage medium may be a tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. The computer storage medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of a computer storage medium would include one or more wire-based electrical connections, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present application.

The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to the invention without departing from the principles thereof are intended to be within the scope of the invention as set forth in the following claims.

Claims

1. A hybrid path planning method based on an improved a algorithm and a dynamic window method, comprising:

2. A hybrid path planning method based on an improved a algorithm and dynamic window method as claimed in claim 1, wherein: the method for achieving global prediction by adopting an A-algorithm to obtain a globally optimal path node comprises the following steps:

3. A hybrid path planning method based on an improved a algorithm and dynamic window method as claimed in claim 2, characterized in that: in steps S1.3 and S1.4, the cost value is calculated by a cost function as follows:

f(n)＝g(n)+w(n)×h(n)

where f (n) represents a cost function from a start point to a node n; g (n) represents the starting pointActual cost to node n; h (n) represents the estimated cost from node n to the target point, h (n) =max (|x) _n -x _g |，|y _n -y _g |)，(x _n ，y _n ) Representing the coordinates of node n, (x) _g ，y _g ) Representing coordinates of a target point; w (n) represents the observation probability of the node n, and is calculated as follows:

4. A hybrid path planning method based on an improved a algorithm and dynamic window method as claimed in claim 2, characterized in that: in step S1.4, the storing N neighborhood nodes around the parent node in the open_list specifically includes:

5. A hybrid path planning method based on an improved a algorithm and dynamic window method as claimed in claim 1, wherein: after the globally optimal path node is obtained, the following processing is performed to delete the redundant path node:

and carrying out further smoothing processing on the generated optimal path.

6. A hybrid path planning method based on an improved a algorithm and dynamic window method as claimed in claim 1, wherein: the method for planning the local path avoiding the obstacle by adopting the dynamic window method comprises the following steps:

s2.7: executing the selected action, and updating the state of the robot;

7. The hybrid path planning method based on the modified a algorithm and the dynamic window method of claim 6, wherein: in step S2.5, the evaluation index of each track is calculated by the following evaluation function:

G(v，ω)＝α×head(v，ω)+γ×path(v，ω)+σ×occ(v，ω)+β×goal(v，ω)

Wherein, the distance between the robot local path end and the target point is evaluated as a sub-function gold (v, omega) by the following method: calculating an overall straight-line distance L between the predicted trajectory end and the target point using Euclidean distance ₁ Predicting the path length L between the end of the trajectory and the target point using Manhattan distance calculation considerations that can only be moved along the grid ₂ Predicting a straight-line distance L between the trajectory end and the target point taking into account the diagonal movement using the diagonal distance calculation ₃ Through L ₁ 、L ₂ 、L ₃ And adding and adjusting the weights of the local path ends to obtain the distance between the local path ends and the target point as an output result of the gold (v, omega).

8. The hybrid path planning method based on the modified a algorithm and the dynamic window method of claim 7, wherein: the evaluation index of each track is normalized to be converted into the same magnitude by head (v, omega), path (v, omega), occ (v, omega) and gold (v, omega) before calculation.

9. A computer readable storage medium storing a computer program, wherein the computer program causes a computer to execute the hybrid path planning method based on the modified a-x algorithm and the dynamic window method according to any one of claims 1-8.

10. An electronic device, comprising: a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the hybrid path planning method based on the modified a-algorithm and dynamic window method according to any one of claims 1-8 when the computer program is executed.