CN112947491A - Unmanned vehicle rapid track planning method - Google Patents

Abstract

The invention discloses a rapid track planning method for an unmanned vehicle, belongs to the field of rapid route planning, and solves the problems of large calculation amount and high hardware requirement of unmanned vehicle track planning in the prior art, and the technical scheme is as follows: constructing an environment map, obtaining coordinate information of a robot moving environment through a sensor to form a map model, and using a grid to pixelate the map; setting the starting point coordinate P₀(x₀,y₀) End point coordinate P₁(x₁,y₁) (ii) a Determining a current point P_i(x_i,y_i) Judging whether the eight neighborhood points of the current point are obstacles or not; and determining the next track point by adopting a midpoint comparison algorithm and an A-Star (A) algorithm according to whether the obstacle is the obstacle. The method has small calculation amount and low requirement on a hardware platform.

Description

Unmanned vehicle rapid track planning method

Technical Field

The invention relates to the field of unmanned vehicle route planning, in particular to a rapid unmanned vehicle trajectory planning method.

Background

The current trajectory planning method comprises an a-Star (a) algorithm, a Dijkstra algorithm, an ant colony algorithm, a genetic algorithm and the like, for example, chinese patent CN 110440822B discloses an automobile welding spot path planning method based on a slime-ant colony fusion algorithm, and chinese patent CN 111098852B discloses a parking path planning method based on reinforcement learning. However, the original a-trace algorithm needs to calculate the distances from the eight adjacent points to the starting point and the ending point, and the calculation amount is still too large, which has a high requirement on a hardware platform. In order to solve the problem of computation amount, the improved A algorithm for providing the jumping point search based on the mobile robot path planning [ J ] robot, 2018,40(6):904 and 910 ] of the improved A algorithm reduces the number of partial useless nodes and reduces partial computation amount, but the computation amount of the reserved nodes is still larger.

Disclosure of Invention

The invention aims to provide a rapid track planning method for an unmanned vehicle, which solves the problem of large calculation amount of track planning of the unmanned vehicle in the prior art, and has the technical effects that: the algorithm has small calculation amount and low requirement on a hardware platform.

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

a method for planning a rapid track of an unmanned vehicle comprises the following steps:

step 1: constructing an environment map, obtaining coordinate information of a robot moving environment through a sensor to form a map model, and using a grid to pixelate the map; step 2: given starting point coordinates P₀(x₀,y₀) End point coordinate P₁(x₁,y₁) (ii) a And step 3: determining a current point P_i(x_i,y_i) When starting, the starting point is the current point, and whether the eight neighborhoods of the current point have obstacles is judged,entering step 4 when no obstacle exists, and entering step 5 when an obstacle exists; and 4, step 4: obtaining the next track point P by the midpoint comparison method_i+1(x_i+1,y_i+1) (ii) a And 5: obtaining next track point P by A-Star method_i+1(x_i+1,y_i+1)；

Further, in step 3, the coordinate determination sequence of the eight neighborhood points is: 1-upper P_i3(x_i,y_i+1), 2-upper right P_i4(x_i+1,y_i+1), 3-Right P_i5(x_i+1,y_i) 4-lower right P_i6(x_i+1,y_i-1), 5-lower P_i7(x_i,y_i-1), 6-lower left P_i8(x_i-1,y_i-1), 7-left P_i1(x_i-1,y_i) 8-upper left P_i2(x_i-1,y_i+1)。

Further, step 4.1: judging the previous track point P_i-1(x_i-1,y_i-1) If there is no obstacle, then using delta x and delta y in last iteration, if there is any, then calculating P_iP₁Increment in x and y directions on a straight line:

step 4.2: judgment of P_iP₁If the region belongs to the region 1, the step 4.3 is carried out, otherwise, the step 4.4 is carried out; step 4.3: judging whether delta x-2 delta y is less than 0, if so, P_i+1The coordinate is (x)_i+1,y_i+1), otherwise P_i+1The coordinate is (x)_i,y_i+1), go to step 4.17; step 4.4: judgment of P_iP₁If the region belongs to the region 2, the step 4.5 is carried out, otherwise, the step 4.6 is carried out; step 4.5: judging whether 2 delta x-delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwise P_i+1The coordinate is (x)_i+1,y_i+1), go to step 4.17; step 4.6: judgment of P_iP₁If the region belongs to the region 3, the step 4.7 is carried out, otherwise, the step 4.8 is carried out; step 4.7: judging whether 2 delta x plus delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwiseP_i+1The coordinate is (x)_i-1,y_i+1), go to step 4.17; step 4.8: judgment of P_iP₁If yes, entering step 4.9, otherwise entering step 4.10; step 4.9: judging whether delta x +2 delta y is less than 0, if so, P_i+1The coordinate is (x)_i-1,y_i+1), otherwise P_i+1The coordinate is (x)_i-1,y_i) Entering step 4.17; step 4.10: judgment of P_iP₁If the region belongs to the region 5, the step 4.11 is carried out, otherwise, the step 4.12 is carried out; step 4.11: judging whether-delta x +2 delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwise P_i+1The coordinate is (x)_i-1,y_i+1), go to step 4.17; step 4.12: judgment of P_iP₁If the region belongs to the region 6, the step 4.13 is carried out, otherwise, the step 4.14 is carried out; step 4.13: judging whether-2 delta x + delta y is less than 0, if so, P_i+1The coordinate is (x)_i-1,y_i-1), otherwise P_i+1The coordinate is (x)_i,y_i-1), go to step 4.17; step 4.14: judgment of P_iP₁If the region belongs to the region 7, the step 4.15 is carried out, otherwise, the step 4.14 is carried out; step 4.15: judging whether-2 delta x-delta y is less than 0, if so, P_i+1The coordinate is (x)_i+1,y_i-1), otherwise P_i+1The coordinate is (x)_i,y_i-1), go to step 4.17; step 4.16: judgment of P_iP₁Belongs to the region 8, further judges whether-delta x-2 delta y is less than 0, if yes, P_i+1The coordinate is (x)_i+1,y_i) Else P_i+1The coordinate is (x)_i+1,y_i-1), go to step 4.17; step 4.17: return P_i+1And (4) coordinates.

Further, in the step 4.1, P is judged_iP₁The judgment method of whether the region belongs to the region 1-the region 8 is as follows:

further, the step 5 includes the following implementation steps:

step 5.1: calculating the current point P_i(x_i,y_i) The euclidean distance from the eight neighborhood points (upper, upper right, lower left and upper left) to the starting point and the end point has the following calculation formula:

step 5.2: constructing a cost function k (n):

k_j(n)＝a_j(n)+b_j(n)

step 5.3: selecting min [ k ]_j(n)]The corresponding point of eight neighborhoods corresponding to the middle j is the next point P_i+1And return to P_i+1And (4) coordinates.

Has the advantages that: the difference between the invention and the prior art is that: when no obstacle exists around eight neighborhood points of the current point, a midpoint comparison method is used for obtaining the next track point, and the next track point can be determined only by calculating few addition operations.

Drawings

FIG. 1: the algorithm of the invention realizes a flow chart;

FIG. 2: eight neighborhood sequence diagrams;

FIG. 3: a midpoint comparison algorithm flow chart;

FIG. 4: regions 1-8 are partitioned;

Detailed Description

The present invention is described in further detail below with reference to the attached drawing figures.

When an environment map is constructed, the mobile environment information of the robot is obtained through a sensor to form a map model, and the map is pixilated by using grids; in engineering, in a small and micro application scene, environmental information can be acquired through a laser radar sensor and a map is constructed, satellite map information can be used in a large scene and used in cooperation with a satellite positioning system, and obstacles and passable areas on the map are explained.

As shown in fig. 1, the search path algorithm rapidly obtains a trajectory between a start point and an end point by using a midpoint comparison method and an a-Star tracing algorithm according to whether an obstacle exists in eight neighborhoods around a current point, which is specifically as follows:

by first setting the starting coordinate to P₀(x₀,y₀) End point coordinate P₁(x₁,y₁) (ii) a Determining a current point P_i(x_i,y_i) Starting point is current point; judging whether the eight neighborhood points of the current point are obstacles, and judging the operation sequence as shown in fig. 2, wherein the judgment is as follows: 1-upper P_i3(x_i,y_i+1), 2-upper right P_i4(x_i+1,y_i+1), 3-Right P_i5(x_i+1,y_i) 4-lower right P_i6(x_i+1,y_i-1), 5-lower P_i7(x_i,y_i-1), 6-lower left P_i8(x_i-1,y_i-1), 7-left P_i1(x_i-1,y_i) 8-upper left P_i2(x_i-1,y_i+1), the judgment result is divided into the presence of obstacles and the absence of obstacles.

(1) When there is no obstacle, the processing flow is shown in fig. 3:

judging whether barriers exist in the previous eight neighborhoods;

if no obstacle exists, the delta x and delta y in the last iteration are used, otherwise, P is calculated_iP₁Increment of the straight line in x and y directions:

the ratio of the increments is slope k ═ Δ y/. DELTA.x.

The linear implicit function f (x, y) is:

f(x,y)＝y-kx-b＝0 (2)

thirdly, constructing a midpoint cost function, as shown in fig. 4, dividing into eight cases:

fourthly, the track acquisition depends on d_iSymbol of (2), independent of d_iI.e. dependent on d_iWhether greater than or equal to 0 or less than 0, if greater than or equal to 0 is + sign, if less than 0 is-sign, in order to reduce floating point operation, it is beneficial to hardware implementation, order d_i＝2△xd_iThe available region 1-8 cost functions are as follows:

according to d_iSelecting the next track point by value, and for the area 1, when d is_i<At 0, the next trace point is P_i+1(x_i+1,y_i+1) when d_iWhen the point is more than or equal to 0, the next track point is P_i+1(x_i+1,y_i). Similarly, the coordinates of the next trace point in the regions 1-8 are as follows:

the effect is that: and the approach point is preferentially selected as a feasible path through midpoint comparison, so that the calculation is rapid and the efficiency is high.

(2) When an obstacle exists, the treatment method comprises the following steps:

calculating a current point P_i(x_i,y_i) The euclidean distance from the eight neighborhood points (left, upper right, lower left) to the starting point and the end point:

constructing a cost function k (n):

k_j(n)＝a_j(n)+b_j(n) (6)

③ selecting min k_j(n)]The corresponding point of eight neighborhoods corresponding to the middle j is the next point P_i+1

The effect is as follows: the most recently optimized route is convenient to select preferentially, and the unmanned vehicle is convenient to avoid the obstacle and select the best route to reach the destination.

The specific working mode is as follows:

further setting the unmanned vehicle in the environment map of the component by constructing the environment map, and setting a starting point coordinate P for the unmanned vehicle₀(x₀,y₀) End point coordinate P₁(x₁,y₁) And determining the current coordinate of the unmanned vehicle as P_i(x_i,y_i) And calculating the moving position according to the mode shown in fig. 1, wherein the moving position is calculated to be the next position point according to a midpoint comparison method and an a-x algorithm in the moving process, specifically: in the process of moving the unmanned vehicle, eight areas are determined by taking the trolley as the center, and under the condition of no obstacle, each area is determined according to d_iThe next coordinate point is judged according to the value of the point P, the unmanned vehicle determines the driving direction according to the starting point, the end point and the series of track points, the next track point coordinate is selected to move constantly, and when an obstacle is met, the current point P is calculated_i(x_i,y_i) To the starting point and the end point, and according to a cost function k_j(n)＝a_j(n)+b_j(n) selecting the minimum value P_i+1As the next moving point.

Has the advantages that: compared with the traditional A-Star tracing algorithm, the method can effectively reduce the calculation amount and reduce the requirement on hardware so as to reduce the hardware cost.

In the present invention, the terms "set", "install", "connect", "fix", etc. should be understood in a broad sense, for example, they may be fixed connection or detachable connection; may be a mechanical connection; can be directly connected, and the specific meaning of the terms in the invention can be understood according to specific situations by a person skilled in the art; the terms "first", "second", and "first" are used for descriptive purposes only and may refer to one or more of such features, and in the description of the invention "plurality" means two or more unless specifically limited otherwise.

The present invention is not limited to the above-described embodiments, and all changes that do not depart from the structure and action of this embodiment are intended to be within the scope of the invention.

Claims (5)

1. The unmanned vehicle rapid track planning method is characterized in that the algorithm comprises the following steps:

step 1: constructing an environment map, obtaining coordinate information of a robot moving environment through a sensor to form a map model, and using a grid to pixelate the map;

step 2: given starting point coordinates P₀(x₀,y₀) End point coordinate P₁(x₁,y₁)；

And step 3: determining a current point P_i(x_i,y_i) When starting, the starting point is the current point, whether an obstacle exists in the eight neighborhoods of the current point is judged, the step 4 is carried out when no obstacle exists, and the step 5 is carried out when an obstacle exists;

and 4, step 4: obtaining the next track point P by the midpoint comparison method_i+1(x_i+1,y_i+1)；

And 5: obtaining next track point P by A-Star method_i+1(x_i+1,y_i+1)。

2. The unmanned aerial vehicle rapid trajectory planning method according to claim 2, wherein in the step 3, the order of judging the coordinates of the eight neighborhood points is as follows: 1-upper P_i3(x_i,y_i+1), 2-upper right P_i4(x_i+1,y_i+1), 3-Right P_i5(x_i+1,y_i) 4-lower right P_i6(x_i+1,y_i-1), 5-lower P_i7(x_i,y_i-1), 6-lower left P_i8(x_i-1,y_i-1), 7-left P_i1(x_i-1,y_i) 8-upper leftP_i2(x_i-1,y_i+1)。

3. The unmanned vehicle rapid trajectory planning method according to claim 2, wherein the step 4 comprises the following implementation steps:

step 4.1: judging the previous track point P_i-1(x_i-1,y_i-1) If there is no obstacle, then using delta x and delta y in last iteration, if there is any, then calculating P_iP₁Increment in x and y directions on a straight line:

step 4.2: judgment of P_iP₁If the region belongs to the region 1, the step 4.3 is carried out, otherwise, the step 4.4 is carried out;

step 4.3: judging whether delta x-2 delta y is less than 0, if so, P_i+1The coordinate is (x)_i+1,y_i+1), otherwise P_i+1The coordinate is (x)_i,y_i+1), go to step 4.17;

step 4.4: judgment of P_iP₁If the region belongs to the region 2, the step 4.5 is carried out, otherwise, the step 4.6 is carried out;

step 4.5: judging whether 2 delta x-delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwise P_i+1The coordinate is (x)_i+1,y_i+1), go to step 4.17;

step 4.6: judgment of P_iP₁If the region belongs to the region 3, the step 4.7 is carried out, otherwise, the step 4.8 is carried out;

step 4.7: judging whether 2 delta x plus delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwise P_i+1The coordinate is (x)_i-1,y_i+1), go to step 4.17;

step 4.8: judgment of P_iP₁If yes, entering step 4.9, otherwise entering step 4.10;

step 4.9: judging whether delta x +2 delta y is less than 0, if so, P_i+1The coordinates are(x_i-1,y_i+1), otherwise P_i+1The coordinate is (x)_i-1,y_i) Entering step 4.17;

step 4.10: judgment of P_iP₁If the region belongs to the region 5, the step 4.11 is carried out, otherwise, the step 4.12 is carried out;

step 4.11: judging whether-delta x +2 delta y is less than 0, if so, P_i+1The coordinate is (x)_i,y_i+1), otherwise P_i+1The coordinate is (x)_i-1,y_i+1), go to step 4.17;

step 4.12: judgment of P_iP₁If the region belongs to the region 6, the step 4.13 is carried out, otherwise, the step 4.14 is carried out;

step 4.13: judging whether-2 delta x + delta y is less than 0, if so, P_i+1The coordinate is (x)_i-1,y_i-1), otherwise P_i+1The coordinate is (x)_i,y_i-1), go to step 4.17;

step 4.14: judgment of P_iP₁If the region belongs to the region 7, the step 4.15 is carried out, otherwise, the step 4.16 is carried out;

step 4.15: judging whether-2 delta x-delta y is less than 0, if so, P_i+1The coordinate is (x)_i+1,y_i-1), otherwise P_i+1The coordinate is (x)_i,y_i-1), go to step 4.17;

step 4.16: judgment of P_iP₁Belongs to the region 8, further judges whether-delta x-2 delta y is less than 0, if yes, P_i+1The coordinate is (x)_i+1,y_i) Else P_i+1The coordinate is (x)_i+1,y_i-1), go to step 4.17;

step 4.17: return P_i+1And (4) coordinates.

4. The unmanned aerial vehicle rapid trajectory planning method according to claim 3, wherein in the step 4.1, P is judged_iP₁The judgment method of whether the region belongs to the region 1-the region 8 is as follows:

5. the unmanned vehicle rapid trajectory planning method according to claim 4, wherein the step 5 comprises the following implementation steps:

step 5.1: calculating the current point P_i(x_i,y_i) The euclidean distance from the eight neighborhood points (upper, upper right, lower left and upper left) to the starting point and the end point has the following calculation formula:

step 5.2: constructing a cost function k (n):

k_j(n)＝a_j(n)+b_j(n)

step 5.3: selecting min [ k ]_j(n)]The corresponding point of eight neighborhoods corresponding to the middle j is the next point P_i+1And return to P_i+1And (4) coordinates.

Images