Unmanned vehicle path planning method and system based on improved A*-TEB fusion algorithm

Abstract

The present invention proposes an unmanned vehicle path planning method and system based on an improved A*-TEB fusion algorithm, relates to the field of unmanned vehicle path planning, constructs a static grid map, and specifies the global starting point and the global target point of the unmanned vehicle in the static grid map; adopts the improved A* global path planning method to construct a global optimal path from the global starting point to the global target point in the static grid map; in the process of moving along the global optimal path, continuously intercepts local paths within a preset range from the unmanned vehicle, and replans the local paths, so that the unmanned vehicle continuously moves towards the local target point until reaching the final global target point; the present invention integrates the improved A* algorithm and the improved TEB algorithm, effectively solves the problems of single global path, poor dynamic performance and local path easily falling into local optimum, thereby realizing complementary advantages and improving the path planning efficiency, global optimality and real-time obstacle avoidance performance of the unmanned vehicle.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned vehicle path planning, and in particular relates to an unmanned vehicle path planning method and system based on an improved A*-TEB fusion algorithm. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] In recent years, the rapid development of autonomous driving technology has made driverless car technology the focus of the transportation field; driverless cars are not only widely used in urban transportation, but also in freight, logistics, agriculture and emergency rescue; this technology is seen as a potential solution to improve traffic safety, reduce traffic congestion and improve travel experience.

[0004] In the field of autonomous vehicles, path planning is a critical task; autonomous vehicles must be able to intelligently plan paths to avoid obstacles, take into account vehicle dynamics, and adapt to changing environmental conditions in real time; the quality of path planning directly affects the safety and efficiency of autonomous vehicles.

[0005] Depending on whether the surrounding environment is known, partially known or unknown, it is divided into global and local path planning algorithms; the global path planning algorithm is responsible for planning the entire path from the starting point to the target point, usually planning while considering the entire map or environment. Common algorithms include RRT, Dijkstra, A* algorithm, etc. Such algorithms still have problems such as long path planning time, non-optimal path length, many turning points, and inability to effectively avoid dynamic obstacles; the local path planning algorithm is responsible for real-time planning and adjustment of the path during the movement of the robot or vehicle to cope with dynamic environments and changing situations. Common algorithms include DWA algorithm, TEB algorithm and MPC algorithm. Such algorithms can effectively plan local paths and achieve real-time obstacle avoidance, but there are problems such as the path is not smooth enough and it is easy to fall into local optimality.

[0006] Therefore, the existing unmanned vehicle path planning methods still have problems such as long global path planning time, non-optimal global path, many path turning points and uneven local path planning, which leads to low efficiency of unmanned vehicle path planning. Summary of the invention

[0007] In order to overcome the shortcomings of the above-mentioned prior art, the present invention provides an unmanned vehicle path planning method and system based on an improved A*-TEB fusion algorithm, which integrates the improved A* algorithm and the improved TEB algorithm, effectively solving the problems of a single global path, poor dynamic performance, and local paths easily falling into local optimality, thereby achieving complementary advantages and improving the unmanned vehicle path planning efficiency, global optimality, and real-time obstacle avoidance performance.

[0008] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

[0009] The first aspect of the present invention provides an unmanned vehicle path planning method based on an improved A*-TEB fusion algorithm.

[0010] The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm includes:

[0011] Construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map;

[0012] The improved A* global path planning method is used to construct a global optimal path from the global starting point to the global target point in the static grid map;

[0013] When the unmanned vehicle moves along the global optimal path, it continuously intercepts local paths within the preset range of the unmanned vehicle and replans the local paths, so that the unmanned vehicle continues to move towards the local target point until it reaches the final global target point.

[0014] Among them, the improved A* global path planning method takes the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate value and dynamic exponential weight from the parent node to the target point into the cost function, calculates the cost estimate value of each adjacent child node, selects the adjacent child node with the smallest cost estimate value as the current node, and performs iterative search and calculation until the current node is the global target point.

[0015] Furthermore, the specific construction process of the global optimal path is:

[0016] (1) Setting an open list to store the node information in the static grid map, encapsulating the global starting point as grid node information and putting it into the open list;

[0017] (2) Take the current node from the head of the open list, use the directional 5-neighborhood search method to search for a group of neighboring child nodes of the current node, and calculate the cost estimate of each neighboring child node through the improved cost function, select the neighboring child node with the smallest cost estimate and put it into the open list, and update the information of the neighboring nodes;

[0018] (3) Repeat step (2) until the target node is found, and use the backtracking algorithm to search for the path with the minimum cost to obtain a temporary global path;

[0019] (4) Using the shortest path algorithm, redundant turning points in the temporary global path are removed to obtain the final global optimal path.

[0020] Furthermore, the directional 5-neighborhood search method selects a search neighborhood from the upper neighborhood, lower neighborhood, left neighborhood, and right neighborhood according to the direction between the current node and the global target point, and selects a group of adjacent child nodes in the search neighborhood with the current node as the origin.

[0021] Furthermore, the mathematical expression of the improved cost function is:

[0022] F(n)=G(n)+w(n)*(H(n)+H(n-1))

[0023] Among them, G(n) is the actual cost from the starting point to the current node, H(n) is the estimated cost from the current node to the target node, H(n-1) is the estimated cost from the parent node to the target node, and w(n) is the dynamic index weight;

[0024] The cost estimate is the diagonal distance, and the mathematical expression is:

[0025]

[0026] Among them, (x g ,y g ) and (x n ,y n ) are the target point coordinates and the current point coordinates respectively;

[0027] The mathematical expression of the dynamic index weight is:

[0028] w(n)=e -(H(n)+H(n-1)) +β

[0029] Among them, β is the weight coefficient, which is a constant.

[0030] Furthermore, the shortest path algorithm is specifically:

[0031] (1) Select the key points of the path, including the starting point and the turning point, and put them into the key point set;

[0032] (2) Traverse the key point set and use the Bresenham straight line algorithm to determine whether the previous and next nodes of the current node can be connected into a straight line without passing through obstacles. If so, discard the current point; otherwise, keep it;

[0033] (3) Repeat (2) until the target point is reached, and finally an optimal path is obtained by eliminating redundant turning points.

[0034] Furthermore, the local path within a preset range of the unmanned vehicle is intercepted as follows:

[0035] Based on the global optimal path, the preset distance is intercepted and serialized to obtain a set of pose points to form a pose sequence. The pose sequence is smoothed using the B-spline curve smoothing algorithm to obtain a smooth pose sequence as the local path, with two adjacent pose points as the local starting point and local target point of the local path.

[0036] Furthermore, the local path is replanned by adopting an improved TEB local path planning method, and an acceleration rate constraint is added to the original TEB multi-objective constraint, specifically:

[0037]

[0038]

[0039]

[0040]

[0041] in, They are respectively the linear acceleration rate constraint and the angular acceleration rate constraint, are the linear acceleration change rate and angular acceleration change rate, a i+1 、a i are the linear accelerations of the i+1th node and the ith node, are the angular accelerations of the i+1th node and the ith node, ΔT i is the time interval between adjacent pose points, and is the coefficient, which is a constant.

[0042] The second aspect of the present invention provides an unmanned vehicle path planning system based on an improved A*-TEB fusion algorithm.

[0043] The unmanned vehicle path planning system based on the improved A*-TEB fusion algorithm includes an initialization module, a global planning module, and a local planning module:

[0044] The initialization module is configured to: construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map;

[0045] The global planning module is configured to: use an improved A* global path planning method to construct a global optimal path from a global starting point to a global target point in a static grid map;

[0046] The local planning module is configured to: when the unmanned vehicle moves along the global optimal path, continuously intercept the local path within the preset range of the unmanned vehicle, and replan the local path so that the unmanned vehicle continuously moves towards the local target point until it reaches the final global target point;

[0047] Among them, the improved A* global path planning method takes the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate value and dynamic exponential weight from the parent node to the target point into the cost function, calculates the cost estimate value of each adjacent child node, selects the adjacent child node with the smallest cost estimate value as the current node, and performs iterative search and calculation until the current node is the global target point.

[0048] The third aspect of the present invention provides a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the steps in the unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as described in the first aspect of the present invention.

[0049] The fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a program stored in the memory and executable on the processor. When the processor executes the program, the steps in the unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as described in the first aspect of the present invention are implemented.

[0050] One or more of the above technical solutions have the following beneficial effects:

[0051] The present invention proposes a path planning method for an unmanned vehicle based on an improved A*-TEB fusion algorithm. By integrating the improved A* algorithm with the improved TEB algorithm, the problems of a single global path, poor dynamic performance, and local paths easily falling into local optimality are effectively solved, thereby achieving complementary advantages and improving the path planning efficiency, global optimality, and real-time obstacle avoidance performance of the unmanned vehicle.

[0052] Based on the traditional A* algorithm, the present invention improves the cost function, considers the distance information of the parent node from the target point, improves the real-time performance of path planning, and introduces dynamic index weights to optimize the path as much as possible, improves the neighborhood search scheme, and proposes a directional 5-neighborhood search, so that the searched nodes are further reduced, thereby improving planning efficiency. In view of the problem of many redundant inflection points, a shortest path algorithm is proposed, which can reduce the redundant inflection points of the path and reduce the path length. Therefore, the improved A* algorithm proposed by the present invention is improved in both planning efficiency and path length compared to the traditional A*.

[0053] The improved TEB algorithm used in the present invention adopts a B-spline curve smoothing algorithm to smooth the initial posture sequence, making the local path smoother. At the same time, an acceleration change rate constraint is added to the original multi-objective constraint, making the unmanned vehicle speed and acceleration changes smoother, which can effectively prevent sudden changes in the speed and acceleration of the unmanned vehicle and improve the life of the motor.

[0054] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0056] Figure 1 This is a flow chart of the method of the first embodiment.

[0057] Figure 2 A schematic diagram of a grid map of the first embodiment.

[0058] Figure 3 This is a flowchart of improving the A* algorithm for the first embodiment.

[0059] Figure 4 This is a schematic diagram of the directional 5-neighborhood search method of the first embodiment.

[0060] Figure 5 This is a flowchart of an improved TEB algorithm for the first embodiment.

[0061] Figure 6 The g2o hypergraph model constructed for the first embodiment. DETAILED DESCRIPTION

[0062] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used in the present invention have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0063] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0064] Embodiment 1

[0065] In one or more embodiments, a method for unmanned vehicle path planning based on an improved A*-TEB fusion algorithm is disclosed, comprising the following steps:

[0066] Construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map;

[0067] The improved A* global path planning method is used to construct a global optimal path from the global starting point to the global target point in the static grid map;

[0068] When the unmanned vehicle moves along the global optimal path, it continuously intercepts local paths within the preset range of the unmanned vehicle and replans the local paths, so that the unmanned vehicle continues to move towards the local target point until it reaches the final global target point.

[0069] Among them, the improved A* global path planning method takes the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate value and dynamic exponential weight from the parent node to the target point into the cost function, calculates the cost estimate value of each adjacent child node, selects the adjacent child node with the smallest cost estimate value as the current node, and performs iterative search and calculation until the current node is the global target point.

[0070] The following describes in detail the implementation process of the unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm in this embodiment from five steps. Figure 1 As shown, specifically:

[0071] Step 1: Grid map initialization: With the help of lidar sensor information, a static grid map is constructed, such as Figure 2 shown.

[0072] Specifically, the mathematical model of the static grid map is as follows:

[0073] Size map =width*height

[0074] Among them, Size map is the number of map grids, width and height are the width and height of the map.

[0075] The grid information is saved using a one-dimensional array OGM, which is specifically expressed as:

[0076]

[0077] Among them, index is the index.

[0078] The conversion formula between index and grid coordinate information is:

[0079] index=y*width+x or index=x*height+y

[0080] Among them, x is the grid coordinate of the horizontal axis, and y is the grid coordinate of the vertical axis.

[0081] Step 2: Specify the starting point of the unmanned vehicle in the grid map (x s ,y s ) and the target point (x g ,y g ), namely the global starting point and the global target point.

[0082] Step 3: Based on the grid map information and the location information of the starting point and the target point, the improved A* global path planning method is used to quickly construct a global optimal path from the starting point to the target point.

[0083] like Figure 3 As shown, specifically:

[0084] Step 3.1: Set up an open list (OpenList) to store grid node location information. The list is a priority queue based on the minimum heap, which can ensure that the head of the queue is always the node with the smallest cost estimate, thereby improving search efficiency. At the same time, initialize the cost map CostMap to record the cost estimate of each grid node, and the initial value is infinity;

[0085] Step 3.2: Encapsulate the starting point of the unmanned vehicle into grid node information, including the node index, the current cost estimate, and the parent node information, and put it into the OpenList.

[0086] Step 3.3: Determine whether OpenList is empty or the target point has been reached. If OpenList is empty, planning fails. If the target point has been reached, planning succeeds and ends. Otherwise, continue to execute.

[0087] Step 3.4: Take the node from the head of the OpenList as the current node, use the directional 5-neighborhood search method to search for a set of legal adjacent child nodes of the current node, and calculate the cost estimate of each adjacent child node by improving the cost function F(n), set the parent node of the child node as the current node, put it in the OpenList as the current node next time, and update the cost information of the adjacent nodes in the cost map CostMap;

[0088] The directional 5-neighborhood search method can effectively avoid redundant searches to improve search efficiency, such as Figure 4 As shown, the specific steps are:

[0089] Determine the current point (xc, yc) and the target point (xg ,y g ) direction; according to different directions, select the upper, lower, left and right neighborhoods as the search neighborhoods;

[0090] Specifically, taking the current node as the origin, the coordinates of the 5 upper neighboring nodes are: [1, 0], [1, 1], [0, 1], [-1, 1], [-1, 0]; the coordinates of the 5 lower neighboring nodes are: [1, 0], [1, -1], [0, -1], [-1, -1], [-1, 0]; the coordinates of the 5 left neighboring nodes are: [-1, 0], [-1, -1], [-1, 1], [0, 1], [0, -1]; the coordinates of the 5 right neighboring nodes are: [1, 0], [1, -1], [1, 1], [0, 1], [0, -1]; if the child node is an obstacle, it is discarded, and the 5 node coordinates are selected as a set of legal adjacent child nodes of the current node.

[0091] The improved cost function F(n) can speed up the search process while ensuring the optimal path as much as possible. Its mathematical expression is:

[0092] F(n)=G(n)+w(n)*(H(n)+H(n-1))

[0093] Among them, G(n) is the actual cost from the starting point to the current point, H(n) is the estimated cost from the current node to the target point, and H(n-1) is the estimated cost from the parent node to the target node. The cost estimate uses the diagonal distance, that is:

[0094]

[0095] w(n) is the dynamic exponential weight, and its mathematical expression is as follows:

[0096] w(n)=e -(H(n)+H(n-1)) +β

[0097] Among them, β is the weight coefficient, which is a constant.

[0098] From this, we can see that as the distance to the target point gets closer, the dynamic index weight w(n) gets smaller, and it gets closer to the Dijkstra algorithm, which can effectively ensure the optimality of the path. At the same time, the introduction of parent node information H(n-1) further improves the search efficiency.

[0099] Step 3.5: Continue to execute step 3.3 until the target node is found, and traverse the cost map CostMap through the backtracking algorithm to search for the path with the minimum cost, and finally obtain the global path.

[0100] The specific steps of the backtracking algorithm are:

[0101] First, the neighborhood where the target point is located is used as the starting neighborhood of the backtracking algorithm. The legal neighborhood is searched in the cost map CostMap through the traditional 8-neighborhood search method, and the coordinate point corresponding to the neighborhood with the smallest cost is put into the global path. Then, starting from this neighborhood, the above process is repeated until the starting point is found and the global path is finally obtained.

[0102] Step 3.6: Use the shortest path algorithm to remove redundant turning points in the global path obtained in step 3.5, so as to obtain a better global path. The specific steps are as follows:

[0103] (1) Select the key points of the path, including the starting point and the turning point, and put them into the key point set;

[0104] (2) Traverse the key point set and use the Bresenham straight line algorithm to determine whether the previous and next nodes of the current node can be connected in a straight line without passing through obstacles. If so, discard the current point; otherwise, keep it.

[0105] (3) Repeat (2) until the target point is reached, and a global optimal path is obtained by eliminating redundant turning points.

[0106] Step 4: According to the global optimal path constructed in step 3 and the local real-time obstacle information, a local path within a preset range of the unmanned vehicle is intercepted from the global optimal path. Combined with the real-time dynamic obstacle information, the improved TEB local path planning method is used to construct a local path that can avoid obstacles in real time and is smoother, so that the unmanned vehicle continues to move towards the target point of the local path, such as Figure 5 As shown, the specific steps are:

[0107] Step 4.1: The global optimal path in step 3 is intercepted into a local path, discretized, and smoothed using the B-spline curve smoothing algorithm to obtain several key points of the local path.

[0108] The nodes in the global optimal path are added with pose information to obtain the pose point P i , its mathematical expression of posture point:

[0109]

[0110] Among them, x i ,y i , β i They represent the horizontal axis, vertical axis and angle between the driverless car and the horizontal axis in the map respectively.

[0111] According to the preset distance, a local path is intercepted from the global optimal path, and several pose points P in the local path are i Constitute a series of pose sequences Q:

[0112]

[0113] Let the time interval between two consecutive postures be denoted as ΔT i , we can get a time series τ consisting of n-1 time intervals:

[0114]

[0115] The pose sequence is smoothed using a B-spline curve to obtain the smoothed pose sequence Q smooth :

[0116]

[0117] Among them, P i is the pose point, which is also the control point of the smoothing function; u is the node vector set; N i，k (u) is the k-order (k-1-order) basis function;

[0118] The B-spline basis function uses the DeBoer-Cox recursion formula:

[0119]

[0120] After smoothing, the pose sequence Q smooth Merge with time series:

[0121] B: =(Q smooth , τ)

[0122] Step 4.2: Construct key point and obstacle constraints, velocity, acceleration and acceleration rate constraints, kinematic constraints, and fastest path constraints to construct the objective function f(B) of multiple constraints of TEB.

[0123] The objective functions of the above multiple constraints are weighted and summed to obtain the global constraint function f(B):

[0124] f(B)=∑ k γ k f k (B)

[0125] Where k is the number of objective functions to be optimized; γ k is the weight of a single objective function; f k (B) is a single objective function;

[0126] Furthermore, key point and obstacle constraints, velocity, acceleration and acceleration rate constraints, kinematic constraints, and fastest path constraints are constructed, specifically:

[0127] Construct key points and obstacle constraints. The mathematical expressions of the two are basically the same. The difference is that key points have an attractive effect on the local path pose point sequence, while obstacles have a repulsive effect on the local path pose point sequence. Their mathematical expressions are as follows:

[0128]

[0129]

[0130] Among them, f path represents the key point constraint, f ob represents obstacle constraints, d min，j is the shortest distance from the pose sequence to the global path point sequence or obstacle; The maximum allowed distance from the pose sequence to the global path point sequence; is the minimum allowed distance value of the obstacle in the pose sequence; S is the scaling factor; ∈ represents a small displacement near the constraint value; n is the polynomial coefficient.

[0131] The purpose of constructing speed, acceleration constraints and acceleration change rate constraints is to limit the speed, acceleration and acceleration change rate to a safe range so that the unmanned vehicle can travel more smoothly. In this embodiment, the mathematical expressions of speed, acceleration and acceleration change rate are as follows:

[0132]

[0133]

[0134]

[0135]

[0136]

[0137]

[0138] Among them, vi, wi, ai, They are linear velocity, angular velocity, linear acceleration, angular acceleration, linear acceleration rate of change and angular acceleration rate of change. Then their constraint functions are:

[0139]

[0140]

[0141]

[0142]

[0143]

[0144]

[0145] Constructing kinematic constraints, the unmanned vehicle can only move in the current heading. This kinematic constraint causes the steering motion posture of the unmanned vehicle to be a smooth path composed of arcs. Therefore, the angle of the unmanned vehicle at the current moment is also equal to the angle at the next moment, that is:

[0146]

[0147]

[0148] The mathematical expression of kinematic constraints is:

[0149]

[0150] Construct the fastest path constraint to ensure that the selection of the local path is optimal in time. The mathematical expression of the constraint is:

[0151]

[0152] Step 4.3: According to the multi-objective optimization function f(B) in step 4.2, use the g2o open source graph optimization library to solve the objective function and finally obtain a smooth local path that satisfies the constraints.

[0153] By minimizing the objective function f(B), the optimal time elastic band is found, thereby obtaining the final local path. The mathematical expression is as follows:

[0154]

[0155] Among them, B * It is the optimal solution that satisfies the constraint function after TEB algorithm optimization.

[0156] The main goal of g2o is to solve graph optimization problems, which can be represented by a graph, such as Figure 6 As shown in the figure, nodes represent optimization variables and edges represent constraints; by minimizing the cost function, g2o can estimate the optimal node variable values ​​to satisfy all constraints in the graph.

[0157] Step 5: While the unmanned vehicle is moving, continue to run step 4 until it reaches the global path target point.

[0158] Embodiment 2

[0159] In one or more embodiments, an unmanned vehicle path planning system based on an improved A*-TEB fusion algorithm is disclosed, including an initialization module, a global planning module, and a local planning module:

[0160] The initialization module is configured to: construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map;

[0161] The global planning module is configured to: use an improved A* global path planning method to construct a global optimal path from a global starting point to a global target point in a static grid map;

[0162] The local planning module is configured to: when the unmanned vehicle moves along the global optimal path, continuously intercept the local path within the preset range of the unmanned vehicle, and replan the local path so that the unmanned vehicle continuously moves towards the local target point until it reaches the final global target point;

[0163] Among them, the improved A* global path planning method takes the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate value and dynamic exponential weight from the parent node to the target point into the cost function, calculates the cost estimate value of each adjacent child node, selects the adjacent child node with the smallest cost estimate value as the current node, and performs iterative search and calculation until the current node is the global target point.

[0164] Embodiment 3

[0165] The purpose of this embodiment is to provide a computer-readable storage medium.

[0166] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as described in the first embodiment of the present disclosure.

[0167] Embodiment 4

[0168] The purpose of this embodiment is to provide an electronic device.

[0169] An electronic device includes a memory, a processor, and a program stored in the memory and executable on the processor. When the processor executes the program, the steps in the unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as described in the first embodiment of the present disclosure are implemented.

[0170] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm is characterized by: include: Construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map; The improved A* global path planning method is used to construct a global optimal path from the global starting point to the global target point in the static grid map; When the unmanned vehicle moves along the global optimal path, it continuously intercepts local paths within the preset range of the unmanned vehicle and replans the local paths, so that the unmanned vehicle continues to move towards the local target point until it reaches the final global target point. The improved A* global path planning method uses the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate from the parent node to the target point and the dynamic index weight into the cost function, calculates the cost estimate of each adjacent child node, selects the adjacent child node with the smallest cost estimate as the current node, and performs iterative search and calculation until the current node is the global target point; The specific construction process of the global optimal path is: (1) Setting an open list to store the node information in the static grid map, encapsulating the global starting point as grid node information and putting it into the open list; (2) Take the current node from the head of the open list, use the directional 5-neighborhood search method to search for a group of neighboring child nodes of the current node, and calculate the cost estimate of each neighboring child node through the improved cost function. Select the neighboring child node with the smallest cost estimate and put it into the open list, and update the information of the neighboring nodes; (3) Repeat step (2) until the target node is found, and use the backtracking algorithm to search for the path with the minimum cost to obtain a temporary global path; (4) Using the shortest path algorithm, remove redundant turning points in the temporary global path and obtain the final global optimal path; The mathematical expression of the improved cost function is: F(n)=G(n)+w(n)*(H(n)+H(n-1)) Among them, G(n) is the actual cost from the starting point to the current node, H(n) is the estimated cost from the current node to the target node, H(n-1) is the estimated cost from the parent node to the target node, and w(n) is the dynamic index weight; The cost estimate is the diagonal distance, and the mathematical expression is: Among them, (x g ,y g ) and (x n ,y n ) are the target point coordinates and the current point coordinates respectively; The mathematical expression of the dynamic index weight is: w(n)=e -(H(n)+H(n-1)) +b Among them, β is the weight coefficient, which is a constant.

2. The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm according to claim 1, characterized in that: The directional 5-neighborhood search method selects a search neighborhood from the upper neighborhood, lower neighborhood, left neighborhood, and right neighborhood according to the direction between the current node and the global target point, and selects a group of adjacent child nodes in the search neighborhood with the current node as the origin.

3. The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as claimed in claim 1, characterized in that: The shortest path algorithm is specifically: (1) Select the key points of the path, including the starting point and the turning point, and put them into the key point set; (2) Traverse the key point set and use the Bresenham straight line algorithm to determine whether the previous and next nodes of the current node can be connected into a straight line without passing through obstacles. If so, discard the current point; otherwise, keep it; (3) Repeat (2) until the target point is reached, and finally an optimal path is obtained by eliminating redundant turning points.

4. The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as claimed in claim 1, characterized in that: The local path within the preset range of the unmanned vehicle is intercepted as follows: Based on the global optimal path, the preset distance is intercepted and serialized to obtain a set of pose points to form a pose sequence. The pose sequence is smoothed using the B-spline curve smoothing algorithm to obtain a smooth pose sequence as the local path, with two adjacent pose points as the local starting point and local target point of the local path.

5. The unmanned vehicle path planning method based on the improved A*-TEB fusion algorithm as claimed in claim 1, characterized in that: The local path is replanned by adopting an improved TEB local path planning method, and an acceleration change rate constraint is added to the original TEB multi-objective constraint, specifically: in, They are respectively the linear acceleration rate constraint and the angular acceleration rate constraint, are the linear acceleration change rate and angular acceleration change rate, a i+1 、a i are the linear accelerations of the i+1th node and the ith node, are the angular accelerations of the i+1th node and the ith node, ΔT i is the time interval between adjacent pose points, and is the coefficient, which is a constant.

6. An unmanned vehicle path planning system based on an improved A*-TEB fusion algorithm, based on an unmanned vehicle path planning method based on an improved A*-TEB fusion algorithm as claimed in any one of claims 1 to 5, characterized in that: Including initialization module, global planning module and local planning module: The initialization module is configured to: construct a static grid map and specify the global starting point and global target point of the unmanned vehicle in the static grid map; The global planning module is configured to: use an improved A* global path planning method to construct a global optimal path from a global starting point to a global target point in a static grid map; The local planning module is configured to: when the unmanned vehicle moves along the global optimal path, continuously intercept the local path within the preset range of the unmanned vehicle, and replan the local path so that the unmanned vehicle continuously moves towards the local target point until it reaches the final global target point; Among them, the improved A* global path planning method takes the global starting point as the initial current node, searches for a group of adjacent child nodes of the current node, introduces the cost estimate value and dynamic exponential weight from the parent node to the target point into the cost function, calculates the cost estimate value of each adjacent child node, selects the adjacent child node with the smallest cost estimate value as the current node, and performs iterative search and calculation until the current node is the global target point.

7. An electronic device, comprising: a memory for non-transitory storage of computer readable instructions; as well as a processor for executing the computer readable instructions, When the computer-readable instructions are executed by the processor, the method described in any one of claims 1 to 5 is executed.

8. A storage medium, characterized in that: The computer-readable instructions are non-transitory stored, wherein when the non-transitory computer-readable instructions are executed by a computer, the instructions of the method according to any one of claims 1 to 5 are executed.

Images