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

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 cell grids divided, two types of cell grid identifications are added by considering the obstacle identifications of surrounding grids with different area sizes and taking each cell 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, and the local drivable route is planned in real time based on an A-star algorithm, so that 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 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 and driving the area: taking each unit grid marked as 0 as a central mass point, judging whether all the C, C (C is an odd number) unit grids in L times of vehicle length radius are 0 (under the condition that the determination of L is to meet the installation condition of the current unmanned vehicle multi-sensor, when the route planning fails, the unmanned vehicle can adjust the direction to rescan in the range of the current C, C (C is an odd number) unit grids, and the adjacent C, C and C grids can realize right-angle turning running at low speed), if so, adding a mark of '2' to the unit grids; taking each cell grid marked with 0 as a central mass point, judging whether horizontal (parallel to an X axis) a +1(a is an even number) cell grids (the total length is just larger than or equal to the vehicle width) and vertical b +1(b is an even number) cell grids (the total length is just larger than or equal to the vehicle length) are marked with 0, if so, adding a mark '3' to the cell grids; taking each cell grid marked with 0 as a central mass point, judging whether horizontal (parallel to an X axis) b +1(b is an even number) cell grids (the total length is just larger than or equal to the vehicle length) and vertical (parallel to a Y axis) a +1(a is an even number) cell grids (the total length is just larger than or equal to the vehicle width) are marked with 0, if so, adding a mark '4' to the cell grids; 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;

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 unit grid marks which can be turned in place or directly turned to the adjacent travelable sub-areas and the unit grid marks corresponding to the travelable sub-areas which can just go forward 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 is a cell grid identification process under different drivable sub-regions after input data is read.

Fig. 3 is a route planning process under various drivable sub-area divisions.

Fig. 4 is a process flow when the current map route planning fails.

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; each of the unit grids marked as 0 was set as a center mass point, and 3 unit grids (having a total length just greater than the vehicle width) horizontally (parallel to the X-axis) and 7 unit grids (having a total length just greater than or equal to that) vertically (parallel to the X-axis) were determinedVehicle length) is marked as 0, if yes, a mark '3' is added to the cell grid; taking each cell grid marked as 0 as a central mass point, judging whether 7 cell grids (the total length is just larger than or equal to the length of the vehicle) horizontally (parallel to the X axis) and 3 cells (the total length is just larger than or equal to the length of the vehicle) vertically (parallel to the Y axis) are marked as 0, 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 node has a feasible point (7-7) of a '2' type, 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 is less than the route 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, if so, 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 (7)

1. A real-time planning method (1) 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 multiple sensors such as a laser radar, an ultrasonic radar and a camera in real time and position coordinates (11) of the unmanned vehicle and a final destination; flexibly applying 3 drivable sub-region division methods, considering the distance and the field ground attributes, and planning a local drivable route (12) of the unmanned vehicle in real time based on an improved A-star algorithm; if the route planning fails, executing the replanning strategy (13) when the current map route planning fails.

2. Acquisition related data (11) according to claim 1, characterized by comprising acquisition of raster map data (111), unmanned vehicle current position, final destination coordinates and established route input.

3. The acquisition grid map data (111) according to claim 2, characterized in that the map is composed of unit grids having a side length of 1, an obstacle identification (time scale 0 of an obstacle having no influence on traveling, otherwise 1) of each unit grid, a roll angle, a pitch angle, a roughness, and coordinates of the unit grid identified as 0.

4. The real-time planning of local travelable routes (12) for unmanned vehicles according to claim 1, characterized by mainly comprising 3 travelable sub-area division methods (121) and a local travelable route generation method (122).

5. The method (121) according to claim 4, wherein each cell grid marked as 0 is used as a central mass point, and whether all the C × C (C is odd) cell grids in the L times of the vehicle length radius are 0 (the determination of L should satisfy the current unmanned vehicle multi-sensor installation condition, when the route planning fails, the unmanned vehicle can adjust the direction to rescan in the range of the current C × C (C is odd) cell grids, and the adjacent C × C cell grids can realize right-angle turning driving at low speed), if yes, the mark "2" is added to the cell grid (1211); taking each cell grid marked with 0 as a central mass point, judging whether the horizontal (parallel to the X axis) a +1(a is an even number) cell grids (the total length is just larger than or equal to the vehicle width) and the vertical b +1(b is an even number) cell grids (the total length is just larger than or equal to the vehicle length) are marked with 0, if so, adding a mark '3' to the cell grids (1212); using each cell grid marked as 0 as a central mass point, judging whether horizontal (parallel to an X axis) b +1(b is an even number) cell grids (the total length is just larger than or equal to the vehicle length) and vertical (parallel to a Y axis) a +1(a is an even number) cell grids (the total length is just larger than or equal to the vehicle width) are marked as 0, if so, adding a mark of '4' to the cell grids (1213); the cell grids identified by "2", "3", or "4" are referred to as travelable grids, and the travelable sub-region (1214) is formed by C × (a +1) × (b +1), or (b +1) × (a +1) cell grids having their centroids as 0.

6. The method (122) for generating a local drivable path as claimed in claim 4, characterized in that the drivable path (1221) is planned using the A-x algorithm, mainly on the basis of a grid of the "class 2" type, taking into account the distance and the maximum roll angle, the maximum pitch and the maximum roughness of the cell grids contained in the sub-regions; when planning to a certain grid fails, planning by considering adjacent unit grids of a 3 class or a 4 class according to the direction of the locomotive (1222); 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 (1223).

7. The re-planning strategy (13) when the current map route planning fails according to claim 1, characterized in that, it is determined whether the currently located sub-area can rotate in place, if not, the route needs to be retroactively traced back to the "2" type grid (131) that is closest to the current location and is traveled; if the vehicle can rotate in place, corresponding to the current grid, the vehicle head is rotated to the position facing the final destination, the number of rotation times is added with 1, a map is generated by scanning, and if the number of rotation times is equal to 4, the vehicle moves back to the grid (132) of the type 2 which is closest to the current position and passes through; if the grid which arrives in a retreating way receives the map (the initial value of the rotation times is 1), the rotation direction of the 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 head is rotated by 90 degrees on the basis of the direction angle of the head when the map is most recently planned to be scanned or received, the rotation times are added by 1, and if the current direction can generate a local terminal point, the map is scanned and generated (133).

Images