CN112833904A - Unmanned vehicle dynamic path planning method based on free space and fast search random tree algorithm - Google Patents

Abstract

The invention discloses an unmanned vehicle dynamic path planning method based on free space and a fast search random tree algorithm. A method for constructing a free space on a map containing obstacles is designed by utilizing a graphical method of concave-polygonal-convex decomposition, and an artificial bee colony algorithm is applied to optimize the path to find out a global optimal path. The fast path planning method based on the improved fast search random tree algorithm is realized by adjusting the sampling probability of the random tree nodes. Finally, the invention realizes the global path planning of the dynamic map and the local rapid path planning in the unmanned vehicle traveling process by respectively utilizing a free space method and a rapid search random tree algorithm, and gives consideration to the quality and the speed of the path planning in the dynamic environment.

Description

Unmanned vehicle dynamic path planning method based on free space and fast search random tree algorithm

1. Field of the invention

The invention relates to the field of path planning, in particular to path planning with a map changed in the process of unmanned vehicle traveling in a dynamic environment.

2. Background of the invention

The automatic driving technology is a hot spot direction in the field of artificial intelligence, and in the future society, most vehicles are configured with the automatic driving technology, so that ground traffic is smoother, and the traffic accident rate is lower. As a method for planning a path in a dynamic environment of a key ring, the following objects and requirements must be achieved: 1) the driving path does not collide with the obstacle; 2) the path must connect the starting and ending regions; 3) the path quality is higher; 4) the dynamic programming time is short.

The existing path planning algorithm is difficult to meet the requirements at the same time, for example, the RRT algorithm and the A-star algorithm are easy to sink into a trap area, and the planning speed is slow in a complex environment; the artificial potential field method and the particle swarm algorithm are easy to fall into the local optimal solution; the grid method is susceptible to the influence of grid division precision; the V-map algorithm and tangent method are not applicable to complex maps; the free space method is suitable for complex maps, but because the original free space construction logic is slightly complex and the calculated amount is large, the method is not suitable for local rapid planning in a dynamic environment.

3. Summary of the invention

Due to different characteristics of different path planning algorithms, quick and high-quality response is difficult to realize in a dynamic environment by using a single algorithm. The invention combines the free space method and the fast search random tree algorithm, and provides an algorithm which can not only meet the path planning quality, but also ensure the real-time performance of the path planning in a dynamically changing environment. Before the unmanned vehicle is started, an optimal path which is from a starting point to a terminal point and does not collide with an obstacle is planned according to a known environment, and the path is adjusted in real time by utilizing a local rapid planning algorithm according to environment change conditions in the process of the unmanned vehicle. Therefore, the method can ensure that the path quality is close to the optimal quality, and can also utilize a local quick path planning algorithm to complete quick response.

The invention firstly provides a global optimal path planning algorithm based on a free space method, which is an algorithm based on graphics and can obtain a global optimal path on the basis of matching with an optimization algorithm, and is improved by combining with an artificial bee colony algorithm. And then, a local path planning method based on a fast search random tree algorithm is also provided, and an algorithm capable of carrying out fast path planning under the environment of local simple terrain is obtained by adjusting the sampling probability of random nodes. Finally, the unmanned vehicle path planning method combines the two algorithms to realize unmanned vehicle path planning in a dynamic environment.

4. Description of the drawings

FIG. 1 is a diagram: schematic diagram of a single connected polygon

FIG. 2 is a diagram of: schematic diagram for judging positive and negative of concave-convex vertex and polygon

FIG. 3 is a diagram of: visible point judgment and optimal visible point selection schematic diagram

FIG. 4 is a diagram of: global search initial path schematic

FIG. 5 is a diagram: path neighborhood searching schematic diagram

FIG. 6 is a diagram of: fast search random tree generation process schematic diagram

FIG. 7 is a diagram of: method for generating global optimal path schematic diagram by improving free space method

FIG. 8 is a diagram of: dynamic change diagram of environment

FIG. 9 is a diagram of: improved RRT algorithm generation local path schematic diagram

FIG. 10 is a diagram: schematic diagram of path adjustment

5. Detailed description of the preferred embodiments

5.1 constructing maps as single connected polygons

The basic idea of the free space method is to divide a map into a plurality of free spaces composed of convex polygons, and a concave polygon convex decomposition algorithm is used in the method, so that the map with obstacles is firstly converted into a polygon, and the basic idea and steps are as follows.

As shown in FIG. 1, a local map M₁M₂…M_nIn which there is an obstacle O₁O₂…O_nWhen selecting a point O of the obstacle_iA certain vertex M of the boundary with the map_iConnecting line, is marked as O_iM_i. The vector of which can have two directions

And

suppose two vectors

And

a distance delta D → 0 between the two points, and the top point O of the obstacle_iAnd the vertex M of the map_iBy connecting two vectors, the map becomes a single connected domain map M₁M₂…M_iO_iO_i+1…O_nO₁…O_iM_i…M_n. If there are multiple polygon obstacles in the map, then it is necessaryIt is sufficient that each obstacle is directly or indirectly connected with the map boundary vertex.

The method comprises the following basic steps: if the map boundary vertex sequence is M_pThe set of barrier polygons is X_obsFind out X_obsAll visible points of each salient point of all barriers on map boundary points are calculated, the distance is calculated, a point-to-point connection with the minimum distance is selected, and the barrier vertex sequence is inserted into the map boundary vertex sequence to form a new map boundary M_pnAnd in the set X of obstacles_obsThe obstacle is removed (wherein the definition of the bump and the visible point refers to 5.2.2, 5.2.3). Up to set X_obstacleAnd if the set is an empty set, the multi-connected domain map is completely transformed into the single-connected domain map, and otherwise, the steps are repeated.

5.2 Single connectivity concave polygonal convex decomposition

After the map is converted into a polygon according to the method and the steps, the polygon is decomposed into a plurality of convex polygons according to the following steps:

the method comprises the following steps: firstly, judging the positive and negative of the polygon, entering the next step if the polygon is regular, and otherwise, reversely sequencing the polygon vertex sequence. (see 5.2.1)

Step two: and searching for concave points in the polygon vertexes from the first vertex, wherein if no concave point exists, the polygon is a convex polygon. If the pits exist, the next step is carried out. (reference 5.2.2)

Step three: one of the pits is selected and the visible point of the pit is searched. (reference 5.2.3)

Step four: and if the visible points have pits, selecting the optimal visible points from the pits, and if the visible points have no pits, selecting the optimal visible points from all the visible points.

Step five: and connecting the concave points and the optimal visible points, and decomposing the polygon into two sub-polygons until all the sub-polygons are convex polygons.

5.2.1 determination of Positive and negative of polygon

The positive and negative of the polygon indicate the arrangement order of the polygon vertexes, and if the polygon is arranged counterclockwise, the polygon is positive, and if the polygon is arranged clockwiseThe polygon is negative, and the judging step is: finding out polygonal salient points P_i(the extreme points of the contour must be polygonal bumps). And a bump P_iThe adjacent front and back points are respectively P_i-1And P_i+1The vector of composition is

And

if it is

The polygon is positive. If it is

The polygon is negative. Since the three points are three fixed points of the polygon, the three points are not collinear and do not exist

The situation of (2) is specifically as shown in fig. 2.

5.2.2 determination of Polygon concave-convex vertices

For a regular polygon, any vertex is P_iAnd the vertex P_iThe adjacent front and back points are respectively P_i-1And P_i+1The vector of composition is

And

if it is

Then the vertex P_iIs a salient point; if it is

Then the vertex P_iAre pits. Since the three points are three fixed points of the polygon, the three points are not collinear and do not exist

The case (1).

5.2.3 definition and selection of visible Point

Any vertex P of the polygon_iIt is connected with any point P_jIs connected to line P_iP_jIf P is_iP_jAll inside or above the polygon, the vertex P_jIs converted into P_iThe visible point of (a). The front and the back two points adjacent to the concave point are respectively P_i-1And P_i+1The vector of composition is

And

and

is marked as

If the visible point is P_kThen the vector connecting the visible point and the concave point is

a is

And

the angle of,

the smaller a is, the better the visible point is. As shown in particular in figure 3. 5.3 bee colony algorithm and free space based Global Path planning

5.3.1 basic steps of the bee colony Algorithm

The method comprises the following steps: taking N bees, wherein N/2 is scout bee, and N/2 is follower bee.

Step two: the detecting bees search the honey sources in the search space, the identity is changed into the leading bees, neighborhood search is conducted near the honey sources, and the honey sources with higher quality are selected and reserved.

Step three: and leading the bees to return to the honeycomb shared information, and determining whether to go to the honey source or not by the following bees according to the quality of the honey source.

Step four: and (4) carrying out neighborhood search near the honey source by the following bees going to the honey source, and if the honey source with higher quality is found, eliminating the original honey source and exchanging the identities of the following bees and the leading bees.

Step five: if honey source passes through T_maxAnd (5) if no better honey source is found after the secondary neighborhood search, abandoning the honey source, leading the identity of the bee to become a scout bee, and returning to the step two.

Step six: and if the total search times reach Limit, keeping the current optimal honey source and stopping searching, otherwise, returning to the third step. The honey collection behavior of the bees is the process of finding the optimal path, the position of the honey source corresponds to the feasible path, the yield of the honey source corresponds to the quality of the path, and the honey collection speed corresponds to the solving speed of the algorithm.

In the path planning algorithm, each honey source corresponds to a feasible path, and the quality of the honey source corresponds to the path length. The initial honey source searching and the honey source domain searching process are shown in 5.3.2 and 5.3.4.

5.3.2 Global search initial Path

As shown in fig. 4, the global search finds a set of free links, selects path points in the free links, and links the free links with the start point and the end point to generate an initial path, the quality of which is not necessarily optimal, but the path is certainly feasible. The method comprises the following steps:

the method comprises the following steps: decomposing the convex polygon of the map with free connecting lines as e₁,e₂……e_n。

Step two: judging convex polygon to which starting point and target point belong

If it is

Step six is entered, otherwise step three is entered.

Step three: find belongings

And randomly select one e_iAnd storing the data into a set E.

Step four: find out e_iBelonging to another convex polygon

If it is

Step six is entered. Otherwise belong to

Randomly selecting a dividing line E not belonging to the set E from all the dividing lines_j。

Step five: find out e_jBelonging to another convex polygon

If it is

Step six is entered. Otherwise belong to

Randomly selecting a dividing line E not belonging to the set E from all the dividing lines_j. If e_jIf yes, storing the data into the set E, and repeating the step five, otherwise, emptying the set E and returning to the step three.

Step six: and any point on each segmentation line in the set E is connected with the starting point and the end point in sequence to generate an initial path.

5.3.3 Path cost (Path quality)

Calculating a Path P_i＝{x_init,p_i1,p_i2,…,p_in,x_goalGet the cost offit₁Here, the path cost is the euclidean distance of the path:

5.3.4 neighborhood search feasible Path

As shown in fig. 5, the path is locally adjusted, in the method, the neighborhood search is defined as moving a certain path point on the dividing line where the path point is located, and the method and the steps are as follows: optionally a dividing line E in the set E_pThe path point thereon is P_epDistance e_pTwo end points P_ep1，P_ep2Are respectively d₁，d₂. For path point P_epThe position of (a) is randomly adjusted within a certain range: will P_epTo e_pAny one end point P_epiMoving a random distance d, wherein d is more than or equal to 0 and less than or equal to a, and if r is more than d_iWhen a is d_iIf r is less than or equal to d_iThen, a is r, r is the set fine tuning range, and i is 1, 2.

5.4 fast search random Tree Algorithm fast Generation of local Path

The method for generating the local path based on the RRT algorithm is as follows:

1) the root node of the tree is defined as the starting point of the path.

2) Updating the probability of each point of the map allowed to be sampled, and randomly sampling in a state space to obtain a sampling point x_randAnd judging whether the point is sampled or not according to the sampling probability, if so, entering the step 3), and otherwise, repeating the step 2).

3) Finding the tree node x closest to the sampling point in the search tree_nearest。

4) With tree node x_nearestAs a starting point, along x_nearest x_randTaking a line segment with the length d in the direction to obtain a new node x_new。

5) Connection x_nearestAnd x_newObtaining a new search branch, detecting whether the branch has intersection with the barrier or the threat area, if not, keeping the branchThis tree node x_newGo to step 6), otherwise, abandon the tree node, jump to step 8).

6) At the new node x_newSearching nearby nodes and judging whether more suitable nodes are taken as x_newParent node of, let x_newThe cost of a node to the starting point is less than the current situation. Usually in x_newThe euclidean distances of the nodes to the starting point are compared.

7) Judgment of x_newIf the nearby node is x_newWhether it is more optimal as a parent node, i.e. whether the path cost to the origin is smaller.

8) Repeating steps 2) to 7) until the target point or target area is contained in the search tree.

9) And reversely searching the father node of each tree node along the target point in the search tree until the starting point, thus obtaining a feasible path from the starting point to the end point.

The RRT algorithm tree search process is shown in fig. 6.

In step 2) above, the final purpose of adjusting the sampling probability is to hope that no denser tree nodes are present around the existing tree nodes, thereby increasing the search speed of the algorithm. Each tree node needs to have an impact on the probability that the surrounding state space is sampled. The sampling probability modification method comprises the following steps: given a state space

A new node x_new∈X_freeIn the state space

The coordinates in (1) are (a, b). State space

The coordinate of any position X ∈ X is (c, d), then X and X_newIs (x ', y'), wherein x 'is c-a and y' is d-b. Probability density P at x_xChange to P_x'＝χP_xWherein χ ═ 1-af (x ', y'),

f (x ', y') is a two-dimensional normal distribution function density function. After the probability of each sampled point in the state space is adjusted according to the existing search tree nodes, whether the sampled point is selected or not is judged according to the following method: given a state space

Set of allowed sampled probabilities for points in a map

Any sampling point x_randE is X, then X_randThe probability of being allowed to be sampled is P_xrand(usually 0.1 to 0.3). Randomly taking p ← rand (0,1), if p<P_xrandThen the sample point can be used, otherwise the sample point is reselected.

5.5 Path planning method under dynamic environment

1) According to the known map conditions, a path planning algorithm based on a free space method and an artificial bee colony algorithm is used for solving an optimal path from a starting point to a target point (as shown in figure 7).

2) The unmanned vehicle advances according to the current planned path, and whether the current map state changes is judged at intervals of t. Firstly, judging whether the end point is changed, and jumping to the step 1) if the end point is changed. Then judging whether the current path is blocked by the barrier, if not, optimizing the path again according to the current path node and the stored initial path node; if the obstacle blocks the next step (fig. 8).

3) Two ends of the path blocked by the obstacle are respectively selected as a starting point and an end point of the local path planning, and the local planning is carried out on the path by utilizing an improved RRT algorithm (as shown in figure 9).

4) Replacing the path blocked by the obstacle with the local path re-planned in step 3).

5) Path optimization: and listing the starting point, the nodes and the end point of the path in sequence, sequentially judging whether the connecting line of each node and the node which is separated from the node generates intersection with the obstacle or not, and discarding other nodes between the two points if the connecting line of each node does not generate intersection (as shown in figure 10).

6) And repeating the steps until the unmanned vehicle moves to the target area.

Claims (3)

1. A method for planning the dynamic path of unmanned vehicle based on free space and fast search random tree algorithm features that the free space method and fast search random tree algorithm are used to plan the global path of dynamic map and the local fast path of unmanned vehicle.

The dynamic path planning method mainly comprises the following steps:

1) according to the known map conditions, solving an optimal path from a starting point to a target point by utilizing a path planning algorithm based on a free space method and an artificial bee colony algorithm;

2) the unmanned vehicle advances according to the current planned path, and whether the current map state changes is judged at intervals of t; firstly, judging whether the end point is changed, and jumping to the step 1) if the end point is changed; then judging whether the current path is blocked by the barrier, if not, optimizing the path again according to the current path node and the stored initial path node; if the obstacle blocks the obstacle, jumping to the next step;

3) selecting two ends of a path blocked by an obstacle as a starting point and an end point of local path planning respectively, and performing local planning on the path by using a fast search random tree algorithm;

4) replacing the path blocked by the obstacle with the local path re-planned in the step 3);

5) path optimization: listing a starting point, a node and an end point of the obtained path according to the sequence, sequentially judging whether a connecting line of each node and a node separated from the node generates intersection with the obstacle or not, and discarding other nodes between the two points if the connecting line does not generate intersection;

6) and repeating the steps until the unmanned vehicle moves to the target area.

2. The dynamic path planning method based on the free space and fast search random tree algorithm as claimed in claim 1, wherein a method for constructing a free space on a map containing obstacles is designed by using a graphical method of concave polygon convex decomposition, and an artificial bee colony algorithm is applied to perform path optimization to find out a global optimal path.

The global path planning method mainly comprises the following steps:

1) constructing a free space: firstly, constructing a map into a single connected polygon, and then carrying out convex decomposition on the single connected concave polygon;

2) global search initial path: globally searching to find out a group of free connecting lines, selecting path points in the free connecting lines, and connecting the path points with a starting point and an end point to generate an initial path;

3) neighborhood search feasible path: and carrying out local adjustment and optimization on the paths by using an artificial bee colony algorithm.

3. The dynamic path planning method based on the free space and fast search random tree algorithm as claimed in claim 1, wherein the local fast path planning method based on the improved fast search random tree algorithm is implemented by adjusting the sampling probability of the random tree nodes.

The local path planning method mainly comprises the following steps:

1) the root node of the tree is defined as the starting point of the path.

2) Updating the probability of each point of the map allowed to be sampled, and randomly sampling in a state space to obtain a sampling point x_randAnd judging whether the point is sampled or not according to the sampling probability, if so, entering the step 3), and otherwise, repeating the step 2).

3) Finding the tree node x closest to the sampling point in the search tree_nearest。

4) With tree node x_nearestAs a starting point, along x_nearest x_randTaking a line segment with the length d in the direction to obtain a new node x_new。

5) Connection x_nearestAnd x_newObtaining a new search branch, detecting whether the branch has intersection with the barrier or the threat area, if not, keeping the branchThis tree node x_newGo to step 6), otherwise, abandon the tree node, jump to step 8).

6) At the new node x_newSearching nearby nodes and judging whether more suitable nodes are taken as x_newParent node of, let x_newThe cost of a node to the starting point is less than the current situation. Usually in x_newThe euclidean distances of the nodes to the starting point are compared.

7) Judgment of x_newIf the nearby node is x_newWhether it is more optimal as a parent node, i.e. whether the path cost to the origin is smaller.

8) Repeating steps 2) to 7) until the target point or target area is contained in the search tree.

9) And reversely searching the father node of each tree node along the target point in the search tree until the starting point, thus obtaining a feasible path from the starting point to the end point.

Images