CN114281072A - Unmanned vehicle navigation method based on global path planning - Google Patents

Abstract

The invention discloses an unmanned vehicle navigation method based on global path planning, which comprises the steps of loading a grid map, expanding a grid outwards for the outline of an obstacle on the grid map, and loading the position of an unmanned vehicle; determining a starting point and an end point, connecting the starting point and the end point, sequentially taking a perpendicular bisector from an intersection point of a connecting line of the perpendicular bisector and the barrier, connecting a grid of the intersection of the perpendicular bisector and the barrier with the starting point, judging whether the grid of the intersection of the perpendicular bisector and the barrier is intersected with the starting point, if the grid of the intersection of the perpendicular bisector and the barrier is intersected with the barrier, continuously taking the perpendicular bisector of another two points until the intersection point of the perpendicular bisector and the barrier and the connecting line of the starting point are not intersected with the barrier, taking the current point as a coordinate point where no vehicle advances, recording the current coordinate point as the starting point, and repeating the steps to obtain a final navigation path. Compared with the prior art, the invention plans a closer route on the premise of avoiding obstacles by the unmanned vehicle, improves the working efficiency of the trolley and effectively solves the problem of global path planning of the unmanned vehicle.

Description

Unmanned vehicle navigation method based on global path planning

Technical Field

The invention relates to the technical field of unmanned vehicle positioning and global path planning, in particular to an unmanned vehicle navigation method based on global path planning.

Background

With the development of the unmanned technology, the application range of the unmanned vehicle is wider, and the application scene of the unmanned vehicle is more complicated. The conventional unmanned vehicle path planning has the problem that the planned path comes and goes in the path planning process, for example, if the obstacle is an obstacle in a concave condition, the planned path of the unmanned vehicle is far when the unmanned vehicle performs the path planning, and the range of the unmanned vehicle path planning is too large, so that the unmanned vehicle has more data amount to be processed, consumes more time in the planning process, and has large calculation amount.

In order to enable the unmanned vehicle to finish tasks more efficiently, the patent provides an unmanned vehicle navigation method based on global path planning.

Disclosure of Invention

The purpose of the invention is as follows: aiming at the technical problems, the technical scheme provides the unmanned vehicle navigation method based on the global path planning, so that a closer route is planned on the premise that the unmanned vehicle avoids the obstacle, the working efficiency of the unmanned vehicle is improved, and the problem of global path planning of the unmanned vehicle is effectively solved.

The technical scheme is as follows: the invention discloses an unmanned vehicle navigation method based on global path planning, which comprises the following steps:

step 1: loading a grid map, expanding a grid outwards for the outline of the obstacle on the grid map, expressing the grid by using a color different from that of the obstacle, and loading the position of the unmanned vehicle;

step 2: the starting point of the unmanned vehicle is marked as S₀The destination of the unmanned vehicle is marked as G₀Will S₀And G₀Connected by a straight line, the distance S of the passing obstacle₀Points from near to far are Z₁、Z₂To Z_n；

And step 3: s₀And G₀Crossed Z of connecting line₁Contour, close to S₀End grid of Z₁₁Near G₀End grid of Z₁₂，Z₁₁And Z₁₂The vertical bisector of the barrier expansion grid is Z₁₃When S is₀And Z₁₃When the connecting line of (A) passes through the obstacle, S₀And Z₁₃The grid of intersection of the connecting line with the contour of the obstacle is Z₁₄，Z₁₃And Z₁₄The vertical bisector of the barrier expansion grid is Z₁₅And so on until S₀And Z_1jThe connecting line does not intersect with the obstacle, and is recorded as Z_1jIs G₁，G₁The coordinates are calculated as follows:

wherein M is Z_1jAnd Z_1(j-1)Point on perpendicular bisector to Z_1jAnd Z_1(j-1)Distance of midpoint, when M takes a minimum value, (X)_G1，Y_G1) Is G₁Grid coordinate, (X)_G1，Y_G1) The value of (a) is taken in the grid with the obstacle contour expanded outwards on the grid map;

and 4, step 4: unmanned vehicle slave S₀The unmanned vehicle can travel to eight adjacent grids from the top left corner, marked as A to E respectively, and starts to move from S₀The upward, downward, leftward and rightward traveling distances are denoted as 2, and the lateral upper left, lateral upper right, lateral lower left and lateral lower traveling distances are denoted as 3, S₀To G₁The path planning is as follows:

F＝L+H

in the formula: x_kFor sitting on the current grid KBiao, Y_kIs the ordinate of the current grid K, F is S₀To G₁Is estimated as L is S₀Known distance to K, H being K to G₁The estimated distance of (2); respectively calculating F of eight adjacent grids and keeping the minimum value of the F, and recording the grid of the minimum value of the F as S₁Repeating the above steps to plan S₁To G₁Path of to S_nAnd G₁Until the superposition;

and 5: put the explored points into the Close list, put G_iPut into Temp list, rest unexplored put into Open list:

Closelist＝[S＝(0，F₁，F₁)]；

step 6: when S is_nAnd G₁The end of the plan of the coincident first path segment is recorded as F₁G is₁The grid is used as the starting point of the unmanned vehicle, the steps 2 to 4 are repeated to carry out the second section of path planning, G₁And G₀The end of the coincidence and path planning is recorded as F_n；

And 7: sequentially connecting the sections of paths obtained in the step 6 to obtain a final path S₀G₀：

S₀G₀＝F₁+F₂+F₃+F₄+F_n。

Further, the position of the unmanned vehicle is compared with the laser radar information and the existing map information through self-adaptive Monte Carlo positioning to obtain specific coordinates of the unmanned vehicle.

Further, S₀To G₁Planning a path of S₀To G₁And (4) making a straight line, and preferentially planning a grid through which the straight line passes.

Has the advantages that:

in the global path planning process, the obstacles in the path are considered, the obstacles in the path are marked firstly, and then the obstacles can be effectively avoided by the method of taking the perpendicular bisector in the scheme, particularly, the obstacles in the concave structure can be effectively avoided when an unmanned vehicle encounters the obstacles, the path planning can be realized in a shorter time compared with the original Astar algorithm, and the path planning can be realized in a shorter time compared with the original Dijkstra algorithm.

Drawings

FIG. 1 is a schematic diagram of a path planning workflow of the present invention;

FIG. 2 is a schematic diagram of the path planning of the present invention;

fig. 3 is a schematic diagram of the global path planning of the present invention.

Detailed Description

The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. The described embodiments are only some embodiments of the invention, not all embodiments. Various modifications and improvements of the technical solutions of the present invention may be made by those skilled in the art without departing from the design concept of the present invention, and all of them should fall into the protection scope of the present invention.

The invention discloses an unmanned vehicle navigation method based on global path planning, which takes the path and the obstacle described in fig. 2 as an example for explanation. The method comprises the following steps:

step 1: and loading a grid map, expanding a grid outwards for the outline of the obstacle on the grid map, expressing the grid with a color different from that of the obstacle, loading the position of the unmanned vehicle, and comparing the laser radar information with the existing map information through self-adaptive Monte Carlo positioning to obtain the specific coordinates of the vehicle.

Step 2: the starting point of the unmanned vehicle is marked as S₀The destination of the unmanned vehicle is marked as G₀Will S₀And G₀Connected by a straight line, the distance S of the passing obstacle₀Points from near to far are Z₁、Z₂To Z_n。

And step 3: s₀And G₀Crossed Z of connecting line₁Contour, close to S₀End grid of Z₁₁Near G₀End grid of Z₁₂，Z₁₁And Z₁₂The vertical bisector of the barrier expansion grid is Z₁₃When S is₀And Z₁₃When the connecting line of (A) passes through the obstacle, S₀And Z₁₃The grid of intersection of the connecting line with the contour of the obstacle is Z₁₄，Z₁₃And Z₁₄The vertical bisector of (A) is Z15, and so on until S₀And Z_1jThe connecting line does not intersect with the obstacle, and is recorded as Z_1jIs G₁，G₁The coordinates are calculated as follows:

wherein M is Z_1jAnd Z_1(j-1)Point on perpendicular bisector to Z_1jAnd Z_1(j-1)Distance of midpoint, when M takes the minimum value (X)_G1，Y_G1) Is G₁Grid coordinate, (X)_G1，Y_G1) The values of (d) are taken in the grid on the grid map with the obstacle contour expanded outward.

And 4, step 4: the unmanned vehicle can travel to eight adjacent grids, the standards from the upper left corner are A to E, the traveling distances in the up, down, left and right directions are marked as 2, the traveling distances in the upper side and the lower side are marked as 3, and S is₀To G₁The path planning is as follows:

F＝L+H

in the formula: k_xAs the abscissa of the current grid K, K_yIs the ordinate of the current grid K, F is S₀To G₁Is estimated as L is S₀Known distance to K, H being K to G₁The estimated distance of (2); respectively calculating F of eight adjacent grids and keeping the minimum value of the F, and recording the grid of the minimum value of the F as S₁Repeating the above steps to plan S₁To G₁Path of to S_nAnd G₁Until superposition, S₀To G₁Planning a path of S₀To G₁And (4) making a straight line, and preferentially planning a grid through which the straight line passes.

And 5: put the explored points into the Close list, put G_iPut into Temp list, rest unexplored put into Open list:

Closelist＝[S＝(0，F₁，F₁)]；

step 6: when S is_nAnd G₁The end of the plan of the coincident first path segment is recorded as F₁G is₁The grid is used as the starting point of the unmanned vehicle, the steps 2 to 4 are repeated to carry out the second section of path planning, G₁And G₀The end of the coincidence and path planning is recorded as F_n。

And 7: sequentially connecting the sections of paths obtained in the step 6 to obtain a final path S₀G₀：

S₀G₀＝F₁+F₂+F₃+F₄+F_n。

Taking FIG. 2 as an example, Z in FIG. 2₁₁And Z₁₂The vertical bisector of the barrier expansion grid is Z₁₃When S is₀And Z₁₃When the connecting line of (A) passes through the obstacle, S₀And Z₁₃The grid of intersection of the connecting line with the contour of the obstacle is Z₁₄When S is found₀And Z₁₄Is connected toThe barrier profiles do not intersect. Therefore, remember Z₁₄Is G₁Determining G₁The coordinates of (a). S₀To G₁Planning a path of S₀To G₁And (4) making a straight line, and preferentially planning a grid through which the straight line passes.

Will be G later₁As S₀And continuing to plan the path, wherein the using method is still the method. Finally by S₀To G₁To G₂To G₃Finally to G₀And finishing the path planning.

The above embodiments are merely illustrative of the technical concepts and features of the present invention, and the purpose of the embodiments is to enable those skilled in the art to understand the contents of the present invention and implement the present invention, and not to limit the protection scope of the present invention. All equivalent changes and modifications made according to the spirit of the present invention should be covered within the protection scope of the present invention.

Claims (3)

1. An unmanned vehicle navigation method based on global path planning is characterized by comprising the following steps:

step 1: loading a grid map, expanding a grid outwards for the outline of the obstacle on the grid map, expressing the grid by using a color different from that of the obstacle, and loading the position of the unmanned vehicle;

step 2: the starting point of the unmanned vehicle is marked as S₀The destination of the unmanned vehicle is marked as G₀Will S₀And G₀Connected by a straight line, the distance S of the passing obstacle₀Points from near to far are Z₁、Z₂To Z_n；

And step 3: s₀And G₀Crossed Z of connecting line₁Contour, close to S₀End grid of Z₁₁Near G₀End grid of Z₁₂，Z₁₁And Z₁₂The vertical bisector of the barrier expansion grid is Z₁₃When S is₀And Z₁₃When the connecting line of (A) passes through the obstacle, S₀And Z₁₃The grid of intersection of the connecting line with the contour of the obstacle is Z₁₄，Z₁₃And Z₁₄The vertical bisector of the barrier expansion grid is Z₁₅And so on until S₀And Z_1jThe connecting line does not intersect with the obstacle, and is recorded as Z_1jIs G₁，G₁The coordinates are calculated as follows:

wherein M is Z_1jAnd Z_1(j-1)Point on perpendicular bisector to Z_1jAnd Z_1(j-1)Distance of midpoint, when M takes a minimum value, (X)_G1，Y_G1) Is G₁Grid coordinate, (X)_G1，Y_G1) The value of (a) is taken in the grid with the obstacle contour expanded outwards on the grid map;

and 4, step 4: unmanned vehicle slave S₀The unmanned vehicle can travel to eight adjacent grids from the top left corner, marked as A to E respectively, and starts to move from S₀The upward, downward, leftward and rightward traveling distances are denoted as 2, and the lateral upper left, lateral upper right, lateral lower left and lateral lower traveling distances are denoted as 3, S₀To G₁The path planning is as follows:

F＝L+H

in the formula: x_kIs the abscissa, Y, of the current grid K_kIs the ordinate of the current grid K, F is S₀To G₁Is estimated as L is S₀Known distance to K, H being K to G₁The estimated distance of (2); respectively calculating F of eight adjacent grids and keeping the minimum value of the F, and recording the grid of the minimum value of the F as S₁Repeating the above steps to plan S₁To G₁Path of to S_nAnd G₁Until the superposition;

and 5: put the explored points into the Close list, put G_iPut into Temp list, rest unexplored put into Open list:

Closelist＝[S＝(0，F₁，F₁)]；

step 6: when S is_nAnd G₁The end of the plan of the coincident first path segment is recorded as F₁G is₁The grid is used as the starting point of the unmanned vehicle, the steps 2 to 4 are repeated to carry out the second section of path planning, G₁And G₀The end of the coincidence and path planning is recorded as F_n；

And 7: sequentially connecting the sections of paths obtained in the step 6 to obtain a final path S₀G₀：

S₀G₀＝F₁+F₂+F₃+F₄+F_n。

2. The unmanned aerial vehicle navigation method based on global path planning of claim 1, wherein the position of the unmanned aerial vehicle is obtained by comparing laser radar information with existing map information through adaptive Monte Carlo positioning to obtain specific coordinates of the unmanned aerial vehicle.

3. The unmanned vehicle navigation method based on global path planning of claim 1, wherein S is₀To G₁Planning a path of S₀To G₁And (4) making a straight line, and preferentially planning a grid through which the straight line passes.

Images