CN108444488B

CN108444488B - Unmanned local path planning method based on equal-step sampling A-x algorithm

Info

Publication number: CN108444488B
Application number: CN201810112446.1A
Authority: CN
Inventors: 王一晶; 刘正璇; 左志强
Original assignee: Tianjin University
Current assignee: Tianjin University
Priority date: 2018-02-05
Filing date: 2018-02-05
Publication date: 2021-09-28
Anticipated expiration: 2038-02-05
Also published as: CN108444488A

Abstract

The invention belongs to the field of unmanned path planning. In order to meet the kinematic constraints and actual traffic constraints of vehicles, the invention provides a local path planning method for unmanned vehicles based on the isochronous sampling A* algorithm. The specific steps are as follows: Step 1: define search Step size and search security area; Step 2: Determine the starting point and target point of the path search in the local grid map; Step 3: Establish the Open list and Closed list; Step 4: Solve the cost function of the grid points in the Open list; Step 5: Select the grid point with the smallest cost function value from the Open list; Step 6: Check all the safe adjacent nodes of the current node respectively; Step 7: Process that the current node has no child nodes during the search process; Step 8: Repeat Step 5 ‑7 until the condition is met, return a feasible path, or return a search failure. The present invention is mainly applied to unmanned road situations.

Description

Unmanned local path planning method based on equal-step sampling A-x algorithm

Technical Field

The invention belongs to the field of unmanned path planning, and particularly relates to a method for carrying out local path planning on an unmanned vehicle by using a path search algorithm based on the principle of constant-step sampling and A-star algorithm.

Background

With the development of society, the requirements of people on living quality are higher and higher, automobiles become indispensable vehicles for human life, however, the incidence rate of global traffic accidents is increased sharply due to the increase of the number of automobiles and the weakness of safety awareness of drivers. In the face of increasingly severe traffic safety and traffic congestion problems, the task of constructing intelligent traffic systems is becoming more urgent. Unmanned vehicles have recently received attention from various parties as a key part in constructing intelligent transportation systems. The unmanned vehicle integrates multiple functions of environment perception and positioning, decision planning, motion control and the like, so that eyes, brain and hands of a driver are replaced, and the unmanned vehicle has the advantages of rapid response, safety and reliability in driving and the like. At present, the unmanned technology of some countries such as the United states, the British and the Germany is developed more mature, the unmanned technology of China starts later, and certain gaps exist between the development of some key technologies and the advanced level of the world.

The path planning algorithm of the unmanned vehicle mainly inherits the algorithm in the robot field, such as the a-star algorithm, the RRT algorithm, the artificial potential field method and the like. The A-Star algorithm is the most effective direct search method for solving the shortest path in the static road network, has good robustness and high response speed to environmental information, and is widely applied to various robots. The A-algorithm is an improvement of the Dijkstra algorithm, and has the advantages of high convergence speed, obvious directivity and small search space compared with the original Dijkstra algorithm. However, the traditional basic a-x algorithm relies on the midpoint of the square grid for searching, and the cost function is only a function of the path length (the cost is only related to the length of the path), so that the disadvantages of fixed path direction, insufficient path smoothness, large turning angle and the like exist, and the effect is not ideal in the actual road simulation and the large curve road simulation considering the restriction of the lane line. As early as the 2007 urban challenge contest held by DARPA (the united states advanced research program office), Junior unmanned vehicles developed by stanford university have acquired a sub-army of the race using an improved a-star algorithm. Some scientific research units in China also use the A-algorithm in the path planning of the unmanned vehicle, for example, China university of science and technology proposes a searchable continuous neighborhood A-algorithm-based unmanned vehicle path planning method. In addition, the RRT algorithm is a randomly sampled planning method. Since 1998, the RRT algorithm is widely used in dynamic environments, high-dimensional state environments, and environments with kinetic constraints due to its incremental growth characteristics. The artificial potential field method is a virtual force method proposed by Khatib, and the motion of a robot is designed into a motion in an artificial gravitational field, so that a safe and smooth path is planned. Since the above two methods are not applied in the present invention, they will not be described below.

Although the predecessors have proposed a variety of unmanned path planning algorithms, most algorithms do not satisfy vehicle kinematics constraints, cannot be directly used for vehicle control, and require a large amount of processing work.

Disclosure of Invention

In order to satisfy the kinematic constraints and practical traffic restrictions of automobiles, the following studies are made on the local path planning of unmanned vehicles:

(1) adding the selection of a cost function under the constraint of the lane line;

(2) selecting a cost function with obstacle avoidance requirements;

(3) considering an actual kinematic model of the vehicle, an algorithm A with steering constraints;

(4) searching and selecting a security domain based on an A-path of any point (non-central point) in the grid;

(5) and planning the turning path of the vehicles at the crossroad.

The invention introduces the thought of equal-step sampling into a local path planning strategy, and the unmanned local path planning method based on the equal-step sampling A-x algorithm comprises the following specific steps:

step 1: defining a search step size and a search security domain;

the search step calculation formula is as follows:

wherein Step represents a search Step length, l represents raster graph precision, v represents the running speed of the current vehicle, and T represents local path updating time;

using a circular security domain to determine the radius r of the circular domain_safeIs defined as:

where L is_vehicleFor the length of the vehicle body, max (·) is a function of taking the maximum value, under the condition that a father node of a current search node is known, a circular domain of the current search node and a circular domain of the center point of the current node and the father node of the current node need to be judged, areas covered by the two circular domains are safe if no obstacles exist, and are dangerous areas if obstacles exist, and a cost function of the nodeThe value is infinite;

step 2: determining a starting point and a target point of path search in the local raster image;

the starting point of the path search is the current position of the vehicle, the starting point is (0,0) point under a vehicle-mounted coordinate system, and the vehicle head points to the positive direction of an x axis, so that the position and the direction of the vehicle in each frame of grid image are the same, the current road shape can be judged according to the lane line information in the grid image, a lane line curve can be obtained through secondary fitting, and a current lane center line curve is further obtained;

and step 3: establishing an Open list and a Closed list, wherein the Open list stores all nodes which are generated but not investigated, the Closed list stores nodes which are investigated and added into a path, the last element in the Closed list is a current path searching node,

and 4, step 4: solving a cost function of grid points in an Open list;

f(n)＝K₁g(n)+K₂h(n)+K₃p(n) (4a)

g(n)＝g₁L_acc(n)+g₂D_acc(n) (4b)

h(n)＝h₁L_est(n)+h₂D_est(n) (4c)

where f (n) represents the total cost of the grid points, K₁、K₂And K₃Is three positive weight coefficients, g (n) represents the cumulative cost from the origin to the node, g₁And g₂Is a positive weight coefficient, L_acc(n) is tiredStep cost of product, D_acc(n) is the accumulated steering cost,

h (n) represents the estimated cost from the node to the destination, h₁And h₂Is a positive weight coefficient, L_est(n) is the estimated step cost, D_est(n) is the estimated steering cost,

p (n) is a penalty term defined as a fixed cost from the parent node to the node, since the distance cost is not different in this term, the portion is determined only by the steering angle, α₁Is a positive weighting factor. θ (n) represents the direction of motion of the current node.

For the starting point, the motion direction θ (0) is 0, the cumulative cost g (0) is 0, and the penalty term p (0) is 0, so the cost function of the starting point is:

f(0)＝K₂(h₁L_est(0)+h₂D_est(0)) (5)

and 5: selecting a grid point with the minimum cost function value from the Open list, marking the grid point as a current node, and moving the grid point into a Closed list from the Open list;

step 6: respectively inspecting all safe adjacent nodes of the current node, if the point is not in an Open list or a Closed list, adding the point into the Open list, and solving a cost function value of the point, wherein the current node is a father node of the point;

and 7: if the current node has no child node in the searching process, the current node returns to the parent node of the current node, the parent node is deleted from the Closed list, and the node with the minimum cost function value is selected again from the Open list;

and 8: repeating the process of the steps 5-7 until the condition is met:

and finishing the search and returning a feasible path after the target point is reached. If the Open list and the Closed list are both empty in the searching process, no feasible path exists, and searching failure is returned.

The specific operation flow of the step 3 is as follows:

adding a starting point into an Open list and a Closed list;

secondly, adding the security grid points adjacent to the starting point into an Open list, wherein the starting point is a father node of the adjacent grid points, and considering

Taking into account the steering constraints of the vehicle, the coordinates (x) of the adjacent grid points_cur,y_cur) Comprises the following steps:

wherein (x)_father,y_father) Representing the coordinates of the parent node, N determining the number of selected nodes, i.e. by a fixed angular difference

Selecting 2N +1 adjacent grid points, phi_maxThe maximum front wheel slip angle of the vehicle is shown, i is a node count, the positive and negative of which determine the steering of the vehicle, i is a positive number, the vehicle appears to turn right, and i is a negative number, the vehicle appears to turn left. The moving direction of the adjacent grid points is also the moving direction

K is shown as the cost function in step 4₁、K₂、K₃、g₁、g₂、h₁、h₂And alpha₁Are all positive and real, and need to be determined by parameter setting, at K₁＝0.8,K₂＝1.52,K₃＝0.25,g₁＝1,g₂＝1.2,h₁＝1,h₂＝0.6,α₁The effect meets the practical requirement when the temperature is 1.2.

Compared with the prior art, the invention has the technical characteristics and effects that:

the invention provides the method for selecting the search node by the equal-step sampling, the motion direction of the node is limited when the adjacent node is determined, the steering constraint of the vehicle is met, the searched path is smooth, and the vehicle control does not need to fit the path under the condition of constant speed. The conventional a-algorithm and some of the improved a-algorithms proposed by the predecessors have the common problems that the distances between nodes in the path are not equal, and the searching direction is not limited, so that the searched path needs a large amount of processing for vehicle control, such as node removal, curve fitting and the like.

Compared with the prior art, the accumulated cost and the estimated cost in the cost function provided by the invention not only comprise the distance cost, but also comprise the angle cost, the cost function is provided with a penalty term and is defined as the fixed cost from the current node to the next node, and the term is only determined by the steering angle because the distance cost is not different in the term. The factors can avoid large turning as much as possible, so that the path is smoother and the mechanical loss of the vehicle is reduced. And at the end of obstacle avoidance, setting the estimated angle cost to enable the path to return to the lane where the vehicle is supposed to run as soon as possible. In order to reduce the calculation complexity, the distance cost part in the cost function adopts the Manhattan distance, and experiments prove that the search time for adopting the Manhattan distance is obviously shorter than the search time for adopting the Euclidean distance.

Because the search point of the invention is not in the center of the grid, the security domain is not suitable by adopting the prior square neighborhood, the security check of the square neighborhood to the direction is not accurate, and the circular neighborhood is more suitable as the security neighborhood because the circular neighborhood has rotational symmetry and is irrelevant to the direction. The safe neighborhood adopted in the invention is all the grid areas covered by two circles which take the current searching node and the central points of the current searching node and the father node thereof as the circle centers and have the diameter of one searching step length and the maximum value in the length of the vehicle body, thereby ensuring that the path can not pass through the barrier and the vehicle can not have collision danger no matter the vehicle runs in any direction.

The method can quickly obtain feasible paths under the road conditions of turning, obstacle avoidance, S-shaped bend and the like at the crossroad through tests, the obtained paths meet the vehicle control limitation, and the search time in Visual Studio is less than 30 ms. (Visual Studio is an integrated development environment for Windows platform applications)

Drawings

FIG. 1 is an overall flow chart of the algorithm of the present invention.

FIG. 2 is a vehicle coordinate system definition.

FIG. 3 is a diagram of the intersection left turn results implemented in Visual Studio 2013.

FIG. 4 is a diagram of the intersection right turn results implemented in Visual Studio 2013.

Fig. 5 is a diagram of the obstacle avoidance result implemented in the Visual Studio 2013.

FIG. 6 is a graph of the S-shaped curved road result implemented in Visual Studio 2013.

Detailed Description

The invention adds the thought of equal-step sampling into a local path planning strategy, and provides a new cost function, which comprises the following specific implementation steps:

step 1: defining a search step size and a search security domain;

defining the search step length as the distance of the vehicle advancing in a control period, so that the search step length calculation formula is as follows:

where Step represents the search Step, l represents the raster map accuracy, v represents the current vehicle travel speed, and T represents the local path update time.

Because the vehicle has the possibility of direction change when advancing, in order to ensure that each node in the path is safe, a circular security domain is adopted for judgment, and the radius of the circular domain is as follows:

where L is_vehicleFor the length of the vehicle body, max (·) is a function of taking the maximum value, under the condition that the father node of the current search node is known, the circular domain of the current search node and the circular domain of the center point of the current node and the father node need to be judged, the two circular domains cover areas without obstacles, the safety is realized, and if obstacles exist, the safety is realizedThe object is a dangerous area, and the cost function value of the node is infinite. Compared with a square security search domain, the round security search domain has the advantages of reducing the judgment range and reducing the possibility of path search failure.

the starting point of the path search is the current position of the vehicle, and is (0,0) point in the vehicle-mounted coordinate system, and the vehicle head points to the positive direction of the x axis, as shown in fig. 2, so that the position and the direction of the vehicle in each grid image are the same. The shape of the current road can be judged according to the lane line information in the grid map, a lane line curve can be obtained through quadratic fitting, and then a center line curve of the current lane is obtained. Because the vehicle runs in the direction of increasing x coordinate strictly under the vehicle-mounted coordinate system, the target point of the path search is defined as the position with the maximum x coordinate value of the curve of the center line of the current lane in the grid map range, and if an obstacle exists in the safety range of the position, a point which meets the safety distance from the obstacle is selected as the target point near the position.

And step 3: establishing an Open list and a Closed list, wherein the Open list stores all generated nodes which are not investigated, the Closed list stores nodes which are investigated and added into a path, and the last element in the Closed list is a current path searching node, and the specific operation flow is as follows:

adding a starting point to the Open list and the Closed list.

Secondly, adding the safety grid points adjacent to the starting point into an Open list, wherein the starting point is a father node of the adjacent grid points, and the coordinates (x) of the adjacent grid points are determined by considering the steering constraint of the vehicle_cur,y_cur) Comprises the following steps:

And 4, step 4: solving a cost function of grid points in an Open list;

f(n)＝K₁g(n)+K₂h(n)+K₃p(n) (4a)

g(n)＝g₁L_acc(n)+g₂D_acc(n) (4b)

h(n)＝h₁L_est(n)+h₂D_est(n) (4c)

where f (n) represents the total cost of the grid points, K₁、K₂And K₃Three positive weighting coefficients. g (n) represents the cumulative cost from the origin to the node, g₁And g₂For positive weight coefficients, the cumulative step cost is:

L_acc(n)＝1+L_acc(n-1)

the cumulative steering penalty is:

h (n) represents the estimated cost from the node to the destination, h₁And h₂For a positive weight coefficient, the estimated step cost is:

wherein (x)_goal,y_goal) Representing the coordinates of the target point.

The estimated steering cost is as follows:

wherein arctan (·) is a function of taking the maximum value.

For the starting point, the motion direction theta (0) is 0, the cumulative cost g (0) is 0, the penalty term p (0) is 0, and the estimated cost is:

wherein (x)_start,y_start) Representing the coordinates of the starting point.

The cost function of the starting point is therefore:

f(0)＝K₂(h₁L_est(0)+h₂D_est(0)) (5)

and 8: repeating the process of the steps 5-7 until the condition is met:

The flow chart of the implementation steps of the invention is shown in fig. 1, and the experimental result shows that the algorithm can meet the requirement of keeping a lane to run when no obstacle exists, as shown in fig. 2 and 3, a smoother path can be obtained when a crossroad turns and the lane can be kept when no obstacle exists, as shown in fig. 4 and 5, the obstacle avoidance of a static obstacle can be realized on the basis of meeting the vehicle steering limitation. K is shown as the cost function in step 4₁、K₂、K₃、g₁、g₂、h₁、h₂And alpha₁All are positive real numbers and need to be determined through parameter setting. The difference of the parameters can cause great difference between the searched paths and the searched time for the same road condition, and the experimental data is shown in K₁＝0.8,K₂＝1.52,K₃＝0.25,g₁＝1,g₂＝1.2,h₁＝1,h₂＝0.6,α₁The effect basically meets the practical requirement when the temperature is 1.2.

The above-mentioned embodiments are intended to illustrate the objects, technical solutions and advantages of the present invention in further detail, and it should be understood that the above-mentioned embodiments are only exemplary embodiments of the present invention, and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

The a algorithm is proposed in the following references:

P.E.Hart,N.J.Nilsson,and B.Raphael.A formal basis for the heuristic determination of minimum cost paths in graphs.IEEE Trans.Syst.Sci.and Cybernetics,SSC-4(2):100-107,1968”。

Claims

1. an unmanned local path planning method based on isochronous sampling A* algorithm, is characterized in that, concrete steps are as follows:

Step 1: Define the search step size and search security domain;

The formula for calculating the search step size is:

Step represents the search step size, l represents the grid image accuracy, v represents the current vehicle speed, and T represents the local path update time;

Judging by the circular safe domain, the radius r _safe of the circular domain is defined as:

Here L _vehicle is the length of the vehicle body, and max( ) is the maximum value function. When the parent node of the current search node is known, it is necessary to determine the circular domain of the current search node and the circle between the center point of the current node and its parent node. If there are no obstacles in the area covered by the two circular domains, it is safe, and if there are obstacles, it is a dangerous area, and the cost function value of this node is infinite;

Step 2: Determine the starting point and target point of the path search in the local grid map;

The starting point of the path search is the current position of the vehicle, which is the (0,0) point in the vehicle coordinate system, and the head of the vehicle points to the positive direction of the x-axis. Therefore, the position and direction of the vehicle in each frame of the grid image are the same. The lane line information in the lattice diagram can determine the current road shape, and the lane line curve can be obtained by quadratic fitting, and then the current lane center line curve can be obtained. Since the vehicle travels strictly in the direction of increasing x coordinate in the vehicle coordinate system, Therefore, the target point of the path search is defined as the maximum position of the x-coordinate value of the current lane centerline curve within the range of the grid map. If there is an obstacle in the safety zone of this position, the point near the position that satisfies the safety distance from the obstacle is selected as the position. Target;

Step 3: Create an Open list and a Closed list. The Open list stores all nodes that have been generated but not inspected. The Closed list stores nodes that have been inspected and added to the path. The last element in the Closed list is the current path search node.

Step 4: Solve the cost function of grid points in the Open list;

f(n)=K ₁ g(n)+K ₂ h(n)+K ₃ p(n) (4a)

g(n)=g ₁ L _acc (n)+g ₂ D _acc (n) (4b)

h(n)=h ₁ L _est (n)+h ₂ D _est (n) (4c)

Among them, f(n) represents the total cost of grid points, K ₁ , K ₂ and K ₃ are three positive weight coefficients, g(n) represents the cumulative cost from the starting point to the node, and g ₁ and g ₂ are positive The weight coefficient of , L _acc (n) is the accumulated step cost, D _acc (n) is the accumulated steering cost,

h(n) represents the estimated cost from the node to the target point, h ₁ and h ₂ are positive weight coefficients, L _est (n) is the estimated step cost, D _est (n) is the estimated steering cost,

p(n) is the penalty term, which is defined as the fixed cost from the parent node to the node. Since there is no difference in the distance cost in this term, the penalty term is only determined by the steering angle, α ₁ is a positive weight coefficient, θ ( n) represents the movement direction of the current node;

For the movement direction of the starting point θ(0)=0, the cumulative cost g(0)=0, the penalty term is p(0)=0, so the cost function of the starting point is:

f(0)=K ₂ (h ₁ L _est (0)+h ₂ D _est (0)) (5)

Step 5: Select the grid point with the smallest cost function value from the Open list, mark it as the current node, and move it from the Open list to the Closed list;

Step 6: Check all the safe adjacent nodes of the current node respectively. If the point is neither in the Open list nor in the Closed list, add the point to the Open list, and solve the cost function value. The current node is the value of the point. parent node;

Step 7: If the current node has no child nodes during the search process, the current node returns its parent node, deletes the point from the Closed list, and selects the node with the smallest cost function value in the Open list again;

Step 8: Repeat the process of steps 5-7 until the conditions are met:

It is regarded as reaching the target point, the search ends, and a feasible path is returned. If both the Open list and the Closed list are empty during the search process, there is no feasible path, and the search fails.

2. the unmanned local path planning method based on isochronous sampling A* algorithm as claimed in claim 1, it is characterized in that, step 3 concrete operation process is as follows:

①Add the starting point to the Open list and the Closed list;

②Add the safety grid point adjacent to the starting point to the Open list, then the starting point is the parent node of the adjacent grid point. Considering the steering constraint of the car, the coordinates of the adjacent grid point (x _cur , y _cur ) for:

Among them, (x _father , y _father ) represents the coordinates of the parent node, and N determines the number of selected nodes, that is, with a fixed angle difference

Select 2N+1 adjacent grid points, φ _max represents the maximum front wheel slip angle of the vehicle, i represents the node count, and its positive or negative determines the steering of the vehicle. When i is a positive number, the vehicle behaves as a right turn, and when i is a negative number , the vehicle behaves as a left turn, and the movement direction of the adjacent grid points is also

3. The unmanned local path planning method based on the isochronous sampling A* algorithm as claimed in claim 1, wherein, as shown in the cost function in step 4, K ₁ , K ₂ , K ₃ , g ₁ , g ₂ , h ₁ , h ₂ and α ₁ are all positive real numbers, which need to be determined by parameter tuning. When K ₁ =0.8, K ₂ =1.52, K ₃ =0.25, g ₁ =1,g ₂ =1.2,h ₁ = 1, h ₂ =0.6, α ₁ =1.2, the effect meets the actual requirements.