CN110967032B

CN110967032B - Real-time planning method for local driving route of unmanned vehicle in field environment

Info

Publication number: CN110967032B
Application number: CN201911220761.7A
Authority: CN
Inventors: 武丹凤; 朱纪洪; 叶松发; 肖尧; 王吴凡
Original assignee: Tsinghua University
Current assignee: Tsinghua University
Priority date: 2019-12-03
Filing date: 2019-12-03
Publication date: 2022-01-04
Anticipated expiration: 2039-12-03
Also published as: CN110967032A

Abstract

The invention provides a real-time planning method for a local driving route of an unmanned vehicle in a field environment, which is mainly used for a route planning scene with limited sensing range, real-time generation of a local map and various ground attributes of the unmanned vehicle in the field environment. On the basis of a real-time map with small unit grids divided, two types of unit grid marks are added by considering the obstacle marks of surrounding grids with different area sizes and taking each drivable unit grid as a central point; on the basis, the unit grids corresponding to different identifications are flexibly utilized, the distance and the ground attributes such as the roll angle, the pitch angle and the roughness of the drivable subarea are comprehensively considered, the improved A-star algorithm is utilized, the local drivable route is planned in real time, the success rate of route planning is improved, and the distance and the time cost for reaching the local terminal point are optimized; when the route planning process fails, a route re-planning strategy is designed.

Description

Real-time planning method for local driving route of unmanned vehicle in field environment

Technical Field

The invention belongs to the field of path planning, and particularly relates to a real-time planning method for a local driving route of an unmanned vehicle in a field environment.

Background

Path planning can be divided into global path planning based on prior complete information and local path planning based on sensor information according to the degree of confidence in the environmental information. The path planning comprises two parts of route planning and driving track planning. The route planning aims at a static map, and generates a route which can be used for a main body to drive as the input of dynamic trajectory planning; trajectory planning determines the speed and direction of the subject to reach each actual travel trajectory point. Under the field environment, the unmanned vehicle sensing system senses the environmental information in real time through the sensor, determines the position of the unmanned vehicle and the distribution condition of obstacles in the sensing range of the unmanned vehicle, and generates a local map in real time, so that the optimal route from the current position to the local sub-target point can be obtained by utilizing a planning algorithm. Common local route planning methods include an a-star search algorithm, an artificial potential field method, an ant colony algorithm, a genetic algorithm, a particle swarm algorithm, a quantum particle swarm algorithm, a neural network algorithm and the like. Methods such as an artificial potential field method, an ant colony algorithm, a genetic algorithm and the like are easy to fall into local optimization, and the time efficiency is not high; learning methods such as neural network algorithms require a large number of samples, and are not suitable for local route planning of field unmanned vehicles without defined roads. The A-algorithm can solve the defects, not only can the optimal solution be obtained with great probability, but also the redundant time can be reduced. Therefore, the invention is designed to design a route planning method based on the A-algorithm, but the traditional grid-based A-algorithm has the following problems:

first, the efficient use of travelable areas conflicts with the computational cost. If the grid division is large, the grid is often marked as non-drivable due to small-volume obstacles, other drivable area spaces in the grid are wasted, and the problem of failed route planning is easily caused; the small grid division will increase the calculation cost and affect the planning time.

Secondly, only the distance cost of the route is considered, and the ground attributes (such as roughness, roll angle and pitch angle) in the travelable area are rarely considered, so that a better input route cannot be provided for the next track planning, the speed planning in the track planning is influenced, and the time for reaching the destination is further influenced.

Thirdly, there is a lack of re-planning method research when route planning fails under current grid division. The existing A-x algorithm does not consider that when the current map planning fails, different driving subarea sizes are flexibly applied to perform grid recalibration so as to form a planning route; nor do they consider adjusting the heading or location for real-time map generation and route re-planning.

Disclosure of Invention

The invention provides a real-time planning method for local driving routes of unmanned vehicles in a field environment, aiming at the corresponding scene characteristics of the unmanned vehicles in the field environment during route planning and the problems of the traditional grid-based A-star algorithm. The method is characterized in that a local map generated in real time according to fusion data of multiple sensors such as a laser radar, an ultrasonic radar and a camera and the position coordinates of an unmanned vehicle and a final destination are considered, 3 travelable subarea division methods are flexibly applied, and a local travelable route of the unmanned vehicle is planned in real time based on an A-ray algorithm, and the method comprises the following steps:

s1, designing input data: a local map generated in real time after multi-sensor data fusion is needed, wherein the map is composed of unit grids with the side length of 1; the barrier mark (the time scale of the barrier which does not influence the driving is 0, otherwise, the mark is 1) of each unit grid, and the roll angle, the pitch angle, the roughness and the coordinates of the unit grid marked as 0; coordinates of the unmanned vehicle and the final destination; existing established route data;

s2, judging whether the current map can generate a local end point: judging whether the current map can generate a local terminal or not according to the input data; if the connecting line of the final destination and the coordinates of the unmanned vehicle does not have an intersection point with the local map, namely the angle between the connecting line and the Y positive axis in the vehicle coordinate system is greater than 90 degrees, discarding the current map and driving according to a set route; if no established route exists, the unmanned vehicle needs to adjust the direction or position of the vehicle to sense the environment again to generate a map;

s3, calibrating the drivable grid: taking each unit grid marked as 0 as a central mass point, and judging whether all the C x C unit grids are 0, wherein C is an odd number, the determination method of the C value is that under the installation condition of the current unmanned vehicle multi-sensor, when the route planning fails, the unmanned vehicle can adjust the direction to rescan on the current grid position, and the minimum length value required by the unmanned vehicle to realize the right-angle turning driving at the low speed of the unmanned vehicle on the current grid position is divided by the upward integer value of the grid resolution, if the upward integer value is an even number, the value of C needs to be added by 1, and if the marks of the C x C grids are all 0, the mark of the unit grid is added by '2'; taking each unit grid marked as 0 as a central mass point, judging whether horizontal odd A grids and vertical odd C grids are marked as 0, wherein the value of A is equal to an upward integral value obtained by dividing the vehicle width by the grid resolution, if the integral value is an even number, the value of A needs to be added with 1, and if the marks of A × C grids are all 0, the mark of 3 is added to the unit grid; taking each unit grid marked as 0 as a central mass point, judging whether the horizontal odd number C grids and the vertical odd number A grids are both marked as 0, if so, adding a mark '4' to the unit grid;

s4, generating a local map terminal: selecting a grid which is closest to the final destination and is currently marked with a mark of '2' as a local map terminal point (the grid can turn the direction of the vehicle);

s5, local map route generation: comprehensively considering the distance and the maximum roll angle, the maximum pitch and the maximum roughness of unit grids contained in the sub-area, and planning a drivable path mainly based on the 2-type grids by utilizing an A-star algorithm; when planning to a certain grid fails, planning by considering the adjacent unit grids of the '3' type or the '4' type according to the direction of the vehicle head; if the route planning is successful, the cost of the current route and the cost of the existing route need to be compared, and the optimal route is selected for driving;

s6, replanning action strategy after route planning fails: if the current map route planning fails and no established route exists, a route re-planning strategy is executed.

Compared with the traditional grid one-time division method and the A-algorithm only considering the distance, the method has the advantages that the cell grid marks which can be turned on site or directly turned to the adjacent travelable sub-areas and the cell grid marks corresponding to the travelable sub-areas which can only go forward and can not turn are added; on the basis, the unit grids corresponding to the two types of identifications are flexibly utilized, the distance and the ground attributes such as the roll angle, the pitch angle and the roughness of the drivable sub-area are comprehensively considered, the drivable route of the local map is planned in real time based on an A-star algorithm, the success rate of route planning is improved, and the distance and the time cost for reaching the local terminal point are optimized.

Drawings

Fig. 1 is a flow of a real-time planning method for a local driving route of an unmanned vehicle in a field environment.

Fig. 2 shows an identification process for a grid of travelable units.

Fig. 3 is a route planning process under various driving grids.

Fig. 4 is a process flow when a failure occurs in the route planning process.

Detailed Description

The present invention will now be described more fully hereinafter with reference to the accompanying drawings, which form a part hereof. It should be understood that the embodiments described herein are merely exemplary of the present invention and are not intended to limit the scope of the invention, and any equivalent substitutions or modifications made within the spirit and principle of the present invention should be included in the scope of the present invention.

As shown in fig. 1, the method for planning local driving route of unmanned vehicle in real time in field environment provided by the embodiment of the invention mainly includes 3 steps:

step 1, acquiring related data: acquiring raster map data, the current position of the unmanned vehicle, the final destination coordinate and set route input;

step 2, if the local map end point can not be generated but a given route exists, driving according to the given route; if the local map end point cannot be generated and no established route exists, the unmanned vehicle needs to rotate to scan to obtain a new map, and the step 3 is switched; if the local map terminal point can be generated, planning the route corresponding to the map, namely firstly identifying the drivable unit grids divided by different drivable subareas, generating the grids corresponding to the local map terminal point, then flexibly applying different unit grid identifications, comprehensively considering the distance and various attributes of the drivable subarea ground, and planning the route by using an A-x algorithm, if the route planning is successful, comparing the cost of a newly planned output route with the cost of a set route, selecting the route with low cost for driving, and if the route planning is failed, turning to the step 3;

and 3, replanning when the planning of the current map route fails.

In order to better understand the real-time planning method for the local driving route of the unmanned vehicle in the field environment provided by the embodiment of the invention, the above 3 steps are explained in detail.

The unmanned ground has the width of 2.6 meters and the length of 5.7 meters, and can rotate in place by taking a mass point of the vehicle as the center. Referring to fig. 2, an embodiment of the present invention first provides a process from reading input data to forming cell grid identifiers under different driving sub-area divisions and generating a local map end point, which includes the following steps:

step (1): inputting grid map data, coordinates (starting points) of the unmanned vehicles, coordinates of final destinations and set route data, and stopping route planning if the current position of the unmanned vehicles is the final destination; if not, judging whether an intersection point exists between a connection line of the final destination and the coordinates of the unmanned vehicle and the local map, namely whether the angle between the connection line of the final destination and the coordinates of the unmanned vehicle and the local map is larger than 90 degrees, if so, not generating a local end point, and at the moment, abandoning the current map and turning to the step (2); if yes, go to step (3)

Step (2): if the set route exists, the vehicle runs according to the set route; if no predetermined route exists, turning to the step associated with FIG. 4;

and (3): 1 x 1 (m) identifying each as 0²) Using the cell grid as a central particle, judging whether all the 7 × 7 cell grids including each particle are 0, and if so, adding a mark '2' to the cell grid; taking each cell grid marked as 0 as a central mass point, judging whether 3 horizontal (parallel to the X axis) cell grids (the total length is just larger than the vehicle width) and 7 vertical cell grids (the total length is just larger than or equal to the vehicle length) are marked as 0, if so, adding a mark '3' to the cell grid; taking each cell grid marked with 0 as a central mass point, judging whether 7 horizontal (parallel to an X axis) cell grids and 3 vertical (parallel to a Y axis) grids are marked with 0, and if so, adding a mark '4' to the cell grid; the cell grids marked with "2", "3" or "4" are called travelable grids, and 7 × 7, 3 × 7 or 7 × 3 cell grids with the centroid as 0 form travelable sub-areas under different divisions;

and (4): selecting a grid with a current identifier of '2' nearest to the final destination as a local map terminal (so as to facilitate the feasibility of scanning and generating a map by turning the direction of the vehicle in the next step), and turning to the step shown in fig. 3;

after the local map terminal and the cell grid identifier which can be driven by the unmanned vehicle are determined, referring to fig. 3, the distance and the maximum roll angle, the maximum pitch and the maximum roughness of the cell grid included in the feasible sub-area are considered comprehensively, and the algorithm A is utilized to flexibly apply the 3 types of grid identifiers to plan the drivable route, which comprises the following steps:

putting a starting point into an Open list, setting the starting point as a current Center node, preferentially judging whether a neighbor of the Center code has a feasible point (7-7) of the type 2, if so, turning to the step (2), otherwise, turning to the step (11);

step (2) judging whether the travelable sub-area with the adjacent feasible point as the centroid contains the final point coordinate, if so, outputting the path to the final point coordinate, and finishing path planning; otherwise, putting feasible points of adjacent grids of the center node into the Open list, setting parent nodes of the feasible point grids as the current center node, and storing records;

step (3) judging whether the adjacent grids have local end points, if not, removing the starting points from the Open list, adding the starting points into the Close list, and turning to step (5); if yes, outputting a local path, judging whether a set route exists, and turning to the step (4);

if no established route exists, driving according to the route output by the new map, and finishing the path planning; if the set route exists, judging whether the route cost from the current starting point to the local terminal point of the new map + the route cost from the local terminal point of the new map to the final terminal point < the cost from the current position to the current driving route terminal point + the cost from the local terminal point of the current driving route to the final terminal point is true or not, if true, driving according to the route output by the new map, finishing the route planning, otherwise, driving according to the set route, and finishing the route planning;

and (5) calculating and storing G, H, F values of feasible points in the current Open list. G is the cost of movement from the origin to the specified grid (along the path generated to reach the cell): g (n) ═ G (fon) + D (cn, n), G (fon) is the accumulated G value of the parent node of feasible point n, and the current parent node (center node) is the map starting point, then G (fon) ═ 0, and D (cn, n) is the distance from the current center node to the neighbor feasible point n; h (n) is the Manhattan distance of n to the final destination; f (n) ═ g (n) + h (n);

and (6) calculating the A and C values of the cell grids in all Open lists. A (n) is the roll angle of the travelable sub-region of the mass point centered on the neighbor feasible point n

Pitch angle

Roughness of

The ground attribute values formed after respective normalization (normalization for all cell grids in the Open list),

wherein 0 ≤ α, β, γ ≤ 1, and α + β + γ ═ 1;

0≤θ≤1，

the normalized distance cost, which affects the time to reach the destination,

the normalized ground driving cost influences the speed of the unmanned vehicle and further influences the time for reaching the destination; selecting the node with the minimum C value in the current Open list, putting the node into the Close list, setting the node as a center node, and turning to the step (7);

step (7) if the current center node has no neighbor feasible point, judging whether the current center node has a set route, if so, driving according to the set route, ending route planning, and if not, turning to step (10); if the neighbor feasible point exists, turning to (8);

step (8) traversing the feasible points which are not traversed (if the center node has the 2-type identifier, firstly traversing the 2-type neighbors, if the neighbors are traversed according to the 2-type identifier, traversing according to the currently used identifier), if the feasible points are feasible driving sub-areas of the centroid and contain the final coordinate, outputting the routes to the final coordinate, and finishing the route planning; if the neighbor feasible point is the local terminal, setting the father of the local terminal as the current center node, outputting a route, and turning to the step (4); otherwise, turning to the step (9); after the feasible points of the neighbors are traversed, turning to the step (6);

step (9) judging whether the neighbor feasible point is in Close list, if so, turning to step (8); if not, judging whether the neighbor feasible point is already in the Open list; if yes, assuming that the current center node is the father node of the node, calculating the G ' and F ' values of the node, if the G ' value is smaller than the original G value of the feasible point, setting the father node of the node as the current center node, updating the original G, F value, and turning to the step (8); if not, adding the node into the Open list, calculating and storing an G, H, F value of the node, setting a father node of the node as a current center node, and turning to the step (8);

step (10) judging whether the current center node carries out route planning according to the 3-7 or 7-3 area centroid, if so, turning to the step associated with the figure 4; if not, turning to the step (11);

and (11) planning a route by taking the vehicle head direction as the center of mass of the 3 × 7 or 7 × 3 area according to the vehicle head direction, judging whether the neighbor of the current center node has a feasible point (note that the left and right neighbor cells are not neighbors), if so, turning to the step (2), if not, judging whether an existing route exists, if so, driving according to the existing route, finishing the planning of the current route, and if not, turning to the step related to the graph 4.

When planning a current map route to a certain grid fails and there is no established route, a vehicle needs to rotate in a grid of class 2 of a current or recently-driven turnable head, and the map is generated by rescanning until a drivable route is generated, as shown in fig. 4, which includes the following steps:

step (1) judging whether the current drivable subarea can rotate in place, if not, backing to the nearest 2-type grid from the current position along a reverse tracing route, and turning to step (2); if the rotation can be performed in situ, directly turning to the step (2);

step (2) corresponding to the current grid, judging whether the rotation times (the rotation times need to be stored and belong to members in the 2-type unit grid structure) are equal to 4, if not, turning to step (3); if so, judging whether the initial starting point is reached at present, if so, finishing the planning of the current route, prompting the starting point to select errors, and if not, reversely tracing the route to the nearest 2-type grid to the current position, and turning to the step (2);

step (3) if the map is not received in the grid (the initial value of the rotation times is 0), the vehicle head is turned to the position facing the final destination, the rotation times is added with 1, the map is generated by scanning, the relevant input data is read, and the step (3) is carried out; if a map is received in the grid (the initial value of the rotation times is 1), determining the rotation direction of the vehicle head according to the direction with a small included angle from the final point (the direction is always followed when the grid rotates after the first determination), rotating by 90 degrees on the basis of the direction angle of the vehicle head when the map is most recently planned to be scanned or received, adding 1 to the rotation times, if a local terminal point can be generated in the current direction, starting to scan to generate the map, and turning to the step (3) related to the map 2; otherwise, go to step (2).

The unit grids are re-identified by using the unit grid barrier identification and the drivable subareas in different division ranges with the unit grids as central points, so that the drivable space is effectively utilized, and the success rate of route planning is improved; the distance and the ground property are comprehensively considered, and the time for reaching the terminal point is optimized. In addition, a solution is proposed when the current map route planning fails.

While the foregoing description shows and describes the preferred embodiments of the present invention, it is to be understood that the invention is not limited to the forms disclosed herein, but is not to be construed as excluding other examples and is capable of use in various other combinations, modifications, and environments and is capable of changes within the scope of the inventive concept as expressed herein, commensurate with the above teachings, or the skill or knowledge of the relevant art. And that modifications and variations may be effected by those skilled in the art without departing from the spirit and scope of the invention as defined by the appended claims.

Claims

1. A real-time planning method for a local driving route of an unmanned vehicle in a field environment is characterized by comprising the steps of obtaining a local map generated by fusing data of a laser radar, an ultrasonic radar and a camera in real time and position coordinates of the unmanned vehicle and a final destination; the obtained data comprises grid map data, the current position of the unmanned vehicle, the final destination coordinate and the set route input related data, 3 drivable subarea division methods and a local drivable route generation method are adopted, the distance and the field ground attribute are considered, and the local drivable route of the unmanned vehicle is planned in real time based on an improved A-star algorithm; if the route planning fails, executing a re-planning strategy when the current map route planning fails;

the 3 travelable subarea dividing method comprises the following steps: taking each unit grid marked as 0 as a central mass point, and judging whether all C, C unit grids in L times of vehicle long radius are 0, wherein the L is determined to meet the current unmanned vehicle multi-sensor installation condition, when the route planning fails, an unmanned vehicle can adjust the direction to rescan in the current C, C unit grids, and adjacent C, C unit grids can realize right-angle turning driving at low speed;

if yes, adding a mark '2' to the cell grid; taking each cell grid marked as 0 as a central mass point, judging whether a +1 cell grids parallel to the X axis and b +1 cell grids vertical to the X axis are marked as 0, if so, adding a mark '3' to the cell grid; taking each unit grid marked as 0 as a central mass point, and judging whether b +1 unit grids parallel to the X axis and a +1 unit grids parallel to the Y axis are marked as 0 or not;

if yes, adding a mark '4' to the cell grid; the unit cells identified by "2", "3", or "4" are called travelable cells, and the travelable sub-regions are constituted by C × (a +1) × (b +1), or (b +1) × (a +1) 0 unit cells with their centroids as the travelable cells;

wherein C is an odd number, a is an even number, and b is an even number; the total length of a +1 unit grids parallel to the X axis is just greater than or equal to the vehicle width, and the total length of b +1 unit grids vertical to the X axis is just greater than or equal to the vehicle length; the total length of b +1 unit grids parallel to the X axis is just greater than or equal to the vehicle length, and the total length of a +1 unit grids parallel to the Y axis is just greater than or equal to the vehicle width;

the replanning strategy when the current map route planning fails comprises the following steps: judging whether the current drivable subarea can rotate in place or not, if not, backing to the nearest 2-type grid to the current position along the reverse tracing route; if the vehicle can rotate in place, corresponding to the current grid, rotating the vehicle head to the position facing the final destination, adding 1 to the number of rotation times, scanning to generate a map, and if the number of rotation times is equal to 4, backing to the nearest 2-type grid to the current position; if the grid which arrives in a retreating mode receives the map, wherein the initial value of the rotation times is 1, the rotation direction of the vehicle head is determined according to the direction with a small included angle from the final point, the direction is always followed when the grid rotates after the first determination, the vehicle head rotates 90 degrees on the basis of the direction angle of the vehicle head when the map is to be scanned or received, the rotation times are added with 1, and if the local final point can be generated in the current direction, the map is generated by scanning.

2. The method for planning the local driving route of the unmanned vehicle in real time under the field environment according to claim 1, wherein the obtaining of the grid map data comprises: the map is composed of unit grids with the side length of 1, the barrier mark of each unit grid does not influence the time scale of the barrier of driving to be 0, otherwise, the barrier mark is 1; roll angle, pitch angle, roughness and coordinates of the cell grid identified as 0.

3. The method for planning the local driving route of the unmanned vehicle in the field environment according to claim 1, wherein the method for generating the local drivable route comprises the following steps: comprehensively considering the distance and the maximum roll angle, the maximum pitch and the maximum roughness of the unit grids contained in the sub-area, and planning a drivable path based on the 2-type grids by using an A-star algorithm; when planning to a certain grid fails, planning by considering the adjacent unit grids of the '3' type or the '4' type according to the direction of the vehicle head; and if the route planning is successful, comparing the cost of the current route with the cost of the existing route, and selecting the optimal route for driving.