CN112444263B - Global path planning method and device - Google Patents
Global path planning method and device Download PDFInfo
- Publication number
- CN112444263B CN112444263B CN202011003654.1A CN202011003654A CN112444263B CN 112444263 B CN112444263 B CN 112444263B CN 202011003654 A CN202011003654 A CN 202011003654A CN 112444263 B CN112444263 B CN 112444263B
- Authority
- CN
- China
- Prior art keywords
- point
- determining
- closest
- closest point
- line
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention provides a global path planning method, which comprises the following steps: acquiring a first skeleton line of a passable area in an unstructured road and determining a second skeleton line according to the first skeleton line; acquiring a right-to-travel path of a passable area; determining a plurality of side lines and a plurality of vertexes of the unstructured road according to the second skeleton line and the right-to-travel path; the edge has a length attribute and a direction attribute; before global path planning is carried out, mapping the current position of the vehicle and a target point to a similar structured map; determining a first closest point set on the side line of the first position to the similar structured map and a second closest point set on the side line of the second position to the similar structured map, and determining an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set; and determining the path between the first nearest point with the minimum evaluation function value and the second nearest point as the optimal path. Therefore, the efficiency of global path planning is improved, and the rationality of path search results is improved.
Description
Technical Field
The present invention relates to the field of autopilot technology, and in particular, to a global path planning method and apparatus.
Background
With the development of artificial intelligence technology and modern manufacturing industry, automatic driving technology has gradually moved into people's daily life, and the traveling mode of people is changed silently. Unmanned techniques can be briefly categorized into sensing, predicting, locating, deciding, planning and controlling. Planning refers to path planning, and is mainly divided into global path planning and local path planning. The global path planning aims at reasonably searching the environment area in the determined area, and generating an optimal collision-free path from the current position of the vehicle to any target position in the area after the safety threshold is considered. Compared to structured regions, unstructured regions lack information describing path topology, so efficient global path planning within unstructured regions is more challenging.
The current unmanned global path planning scheme is mainly divided into two types of grid-based graph searching algorithms and random sampling-based searching algorithms:
1. the grid-based graph search algorithm is represented by the a-algorithm and its variants. The main idea is to rasterize the search area and search the surrounding space with one grid as the sampling step. And evaluating and screening each sampling point through a designed heuristic function, and continuing to explore until the target point is explored after preferential selection. After the target point is searched, a final search path is obtained according to the parent node set during the search.
2. The random sampling based search algorithm is represented by the RRT algorithm. The main idea is that the starting point is used as the root node, a random expansion tree is generated by adding leaf nodes through random sampling, and when the leaf nodes in the random tree contain target points or enter target areas, a path from the starting point to the target points can be found in the random tree.
The rationality of a grid-based search algorithm to generate a path depends on the shape and accuracy of the grid. For unstructured areas with large range and complex road conditions, different grid sizes and shapes are needed according to the width and the shape of the passing area, so that the map manufacturing cost is increased, and the calculated amount is increased.
Although the search algorithm based on random sampling does not need to grid the map in advance, the search algorithm is a pure random search algorithm, is insensitive to the environment type, has low convergence speed and greatly reduces the efficiency when a search space contains a large number of obstacles or narrow channel constraints, and is difficult to find a path in the environment with a narrow channel.
Disclosure of Invention
The embodiment of the invention aims to provide a global path planning method and a global path planning device, which are used for solving the problems that the rationality of generating a path by a grid-based search algorithm in the prior art depends on the shape and the precision of the grid, different grid sizes and shapes are needed to be adopted according to the width and the shape of a passing area, the manufacturing cost of a map can be increased, the calculation amount can be increased, the random sampling-based search algorithm is insensitive to the environment type, and when a search space contains a large number of barriers or narrow channel constraints, the convergence speed of the algorithm is slow, the efficiency is greatly reduced, and the path is difficult to find in the environment with a narrow channel.
In a first aspect, the present invention provides a global path planning method, the global path planning method including:
acquiring a first skeleton line of a passable area in an unstructured road; the unstructured road comprises a passable area and an unvented area;
determining a second skeleton line according to the distance between each point in the first skeleton line and the boundary of the unstructured road;
acquiring a right-to-travel path of the passable area;
determining a plurality of side lines and a plurality of vertexes of the unstructured road according to the second skeleton line and the right-to-travel path, wherein the side lines and the vertexes form a similar structured map; the side lines have length attributes and direction attributes, and the intersection point of any two side lines is the vertex of the unstructured road;
before global path planning is carried out, mapping the current position of the vehicle and a target point to the structured map;
determining a first nearest point set from the first position to an edge line of the similar structured map and a second nearest point set from the second position to the edge line of the similar structured map according to a first position of the current position of the vehicle in the similar structured map and a second position of the target point in the similar structured map;
Determining an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set according to the length from each first closest point in the first closest point set to the sampling point along the edge line, the direction from each first closest point to the sampling point along the edge line and the distance from the sampling point to each second closest point;
and determining the path between the first nearest point with the minimum evaluation function value and the second nearest point as the optimal path.
In one possible implementation manner, the acquiring the first skeleton line of the passable area in the unstructured road specifically includes:
rasterizing a passable area in the unstructured road, and dividing the passable area into a plurality of grids;
determining a grid value of each grid according to the distance from each grid to the non-passable area;
and when any grid value represents a different part from the non-passable area, determining a first skeleton line according to the any grid value.
In one possible implementation manner, the acquiring the right-to-travel path of the passable area specifically includes:
acquiring grids adjacent to the non-passable area;
And determining a right-to-travel path according to the grid value of the grids adjacent to the non-passable area.
In one possible implementation, the grid size is determined according to the width of the vehicle when the rasterization process is performed;
the distance from the right-to-travel path to the boundary of the unstructured road is determined based on the width of the vehicle and the minimum turning radius.
In one possible implementation manner, the determining the second skeleton line according to the distance between each point in the first skeleton line and the boundary of the unstructured road specifically includes:
and deleting the point in the first skeleton line to obtain a second skeleton line when the distance between the point in the first skeleton line and the boundary of the passable area is smaller than a preset distance threshold value.
In one possible implementation, the edge has a direction attribute specifically including:
acquiring any point on any side line of a right-to-travel path;
judging whether the first side point and the second side point of any point are in the unstructured road or not; the first side point and the second side point are arranged on two sides of the arbitrary point;
when the first side point is not in the unstructured road, determining the direction attribute of any side line as a first direction, and when the second side point is not in the unstructured road, determining the direction attribute of any side line as a second direction; the first direction and the second direction are opposite to each other.
In one possible implementation manner, the searching the first closest point set on the edge line of the first position to the similar structured map, and the second closest point set on the edge line of the second position to the similar structured map specifically includes:
determining a starting circle by taking the first position as a circle center and taking a preset first radius as a radius;
determining a first nearest point set in the starting circle, and taking each first nearest point in the first nearest point set as a starting point of global path planning;
determining an end point circle by taking the second position as a circle center and taking a preset second radius as a radius;
and determining a second closest point set in the end point circle, and taking each second closest point in the second closest point set as an end point of global path planning.
In one possible implementation manner, the determining the evaluation value of each first closest point in the first closest point set to each second closest point in the second closest point set according to the length from each first closest point in the first closest point set to the sampling point along the edge, the direction from each first closest point to the sampling point along the edge, and the distance from the sampling point to each second closest point specifically includes:
Determining the length from each starting point to the sampling point along the edge line;
determining the direction from each starting point to the sampling point along the side line;
determining a first product of a preset first weight value and the length;
determining a second product of a preset second weight value and the direction;
determining a sum of the first product and the second product as a first generation value;
determining the distance from the sampling point to the target point as a second cost value;
and determining an evaluation function value according to the first generation value and the second generation value.
In a second aspect, the present invention provides a global path planning apparatus, the global path planning apparatus comprising:
the acquisition unit is used for acquiring a first skeleton line of a passable area in the unstructured road; the unstructured road comprises a passable area and an unvented area;
a determining unit for determining a second skeleton line according to a distance of each point in the first skeleton line from a boundary of the unstructured road;
the acquisition unit is also used for acquiring a right-to-travel path of the passable area;
the determining unit is further configured to determine a plurality of edges and a plurality of vertices of the unstructured road according to the second skeleton line and the right-to-travel path, where the plurality of edges and the plurality of vertices form a similar structured map; the side lines have length attributes and direction attributes, and the intersection point of any two side lines is the vertex of the unstructured road;
The mapping unit is used for mapping the current position of the vehicle and the target point to the structured map before global path planning is carried out;
the determining unit is further configured to determine a first closest point set from a first position of a current position of the vehicle in the generic structured map and a second closest point set from the first position to an edge of the generic structured map, and from the second position to a second closest point set on the edge of the generic structured map, according to the first position of the current position of the vehicle in the generic structured map and the second position of the target point in the generic structured map;
the determining unit is further configured to determine an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set according to a length from each first closest point in the first closest point set to the sampling point along the edge line, a direction from each first closest point to the sampling point along the edge line, and a distance from the sampling point to each second closest point;
the determining unit is further configured to determine that a path between a first closest point to a second closest point where the evaluation function value is smallest is an optimal path.
In a third aspect, the invention provides an apparatus comprising a memory for storing a program and a processor for performing the method of any of the first aspects.
In a fourth aspect, the present invention provides a computer program product comprising instructions which, when run on a computer, cause the computer to perform the method according to any of the first aspects.
In a fifth aspect, the present invention provides a computer readable storage medium having a computer program stored thereon, which when executed by a processor, implements a method according to any of the first aspects.
By applying the global path planning method and the global path planning device provided by the embodiment of the invention, unstructured roads can be subjected to topological processing to obtain the similar structured map, so that the efficiency of global path planning is improved through global path planning of the similar structured map, and meanwhile, when the path evaluation is carried out, the length attribute and the direction attribute of the boundary in the similar structured map can be combined, so that the rationality of the path search result is improved.
Drawings
FIG. 1 is a flow chart of a global path planning method according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of an unstructured road according to a first embodiment of the present invention;
FIG. 3 is a schematic diagram of a class structured map for generating unstructured roads according to an embodiment of the present invention;
FIG. 4 is a block diagram of FIG. 3;
fig. 5 is a first skeleton line of a sub-structured road according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of a second frame wire and right-to-travel path according to an embodiment of the present invention;
FIG. 7 is a schematic diagram of a direction attribute of a border according to a first embodiment of the present invention;
FIG. 8 is a schematic diagram of an unstructured road with directional attribute according to an embodiment of the present invention;
FIG. 9 is a schematic diagram of a first closest point set and a second closest point set according to a first embodiment of the present invention;
fig. 10 is a schematic structural diagram of a global path planning apparatus according to a second embodiment of the present invention.
Detailed Description
The present application is described in further detail below with reference to the drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting of the invention. It should be noted that, for convenience of description, only the portions related to the present invention are shown in the drawings.
It should be noted that, in the case of no conflict, the embodiments and features in the embodiments may be combined with each other. The present application will be described in detail below with reference to the accompanying drawings in conjunction with embodiments.
Fig. 1 is a flowchart of a global path planning method according to an embodiment of the present invention. The execution subject of the present application is a terminal, server or processor with computing functionality. The application will be described by taking the application of the method to an unmanned vehicle as an example, and when the method is applied to an unmanned vehicle, the execution subject of the method is an autonomous vehicle control unit (Automated Vehicle Control Unit, AVCU), i.e., a central processing unit of the unmanned vehicle corresponds to the "brain" of the unmanned vehicle. The application comprises the following steps:
Specifically, while the autonomous vehicle is traveling, a map of the area being traveled is loaded, in which there are structured roads and unstructured roads. The unstructured road is opposite to the structured road, the structured road refers to a highway with better structure such as expressway and urban arterial road, the road has clear road mark lines, the background environment of the road is single, and the geometric characteristics of the road are obvious. For the structured roads, the topological relation among the paths can be easily obtained, and then the global path planning is carried out according to the topological relation.
Unstructured roads generally refer to roads with low structuring degree such as urban non-arterial roads, rural streets and the like, the roads have no lane lines and clear road boundaries, and the road areas and the non-road areas are difficult to distinguish under the influence of shadows, water tracks and the like. The unstructured road includes a passable area and an unvarying area.
Referring to fig. 2, fig. 2 is a schematic view of an unstructured road provided in an embodiment of the present invention, wherein a lower point in fig. 2 may be used as a current location of a vehicle, and an upper point may be used as a target point of the current vehicle. The dark areas are passable areas and the white areas are non-passable areas.
For unstructured roads like in fig. 2, its first skeleton line may be determined by a certain processing. The process here is a rasterizing process, and the step of determining the first skeleton line by the rasterizing process is as follows:
firstly, rasterizing a passable area in an unstructured road, and dividing the passable area into a plurality of grids; secondly, determining a grid value of each grid according to the distance from each grid to the non-passable area; finally, when any grid value represents a different part in the distance non-passable area, determining the first skeleton line according to any grid value.
How to determine the first skeleton line is described below in connection with fig. 3.
In fig. 3, the black area is an unvented area, the gray area is an unvented area, the unvented area is regarded as an obstacle, the boundary of the unstructured road in fig. 3 is regarded as a single obstacle, the unvented area is rasterized, each raster value represents the distance to the nearest obstacle, the raster value of the raster occupied by the obstacle is 0, the raster values of the raster adjacent to the obstacle are recorded as 1, and the raster values of the raster adjacent to the obstacle are sequentially extended outwards until the two different obstacles are extended to the end of the same raster, for example, the raster value is 4, the raster with the raster value of 4 represents that the two different obstacles are extended to the same raster, and at the moment, the extended and terminated raster forms the first skeleton line of the area.
Fig. 4 is a block diagram of fig. 3, and referring to fig. 4, the passable area may be divided into 5 blocks according to white lines, and for zone 1, the obstacle closest to the point of zone 1 is the left obstacle, and none of the other obstacles is the left obstacle. For zone 2, the obstacle closest to zone 2 is the upper obstacle, and none of the other obstacles is closer to the upper obstacle. Similarly, for zone 5, the obstacle closest to zone 5 is the middle obstacle, and none of the other side obstacles is closer than the middle obstacle.
Therefore, the connection line between the grid and the vertex of the grid value 4 is used for distinguishing different blocks, the point on the upper side of the connection line between the grid and the vertex of the grid value 4 is closer to the obstacle on the upper side, the point on the lower side of the connection line is closer to the obstacle on the left side, and the connection line is just a dividing line, so that the connection line is used as a skeleton line.
Similarly, the center point is taken by the grid of the grid value 4, and the center point is the same distance from the left side, the upper side and the middle barrier, so the center point is taken as a demarcation point.
Thus, according to fig. 3 and 4, the first skeleton line of the unstructured road in fig. 2 may be determined, the first skeleton line of the unstructured road in fig. 2 is shown in fig. 5, and fig. 5 is the first skeleton line of the split structured road according to the first embodiment of the present invention.
The size of the grid during the rasterization process is determined according to the width of the vehicle, for example, the size of the grid and the width of the vehicle are in a fixed proportion, for example, 1:1, or the size of the grid is determined according to the length and the width of the vehicle, for example, the size of the grid is determined by squaring the length of the vehicle plus the square of the width and then squaring, so that the size of the grid is determined according to the length and the width of the vehicle, and the speed during the grid process is convenient to increase.
Specifically, for the first skeleton line, the first skeleton line is composed of a plurality of side lines, and when the distance between a point on the side line in the first skeleton line and the boundary of the passable area is smaller than a preset distance threshold value, the side line in the first skeleton line is deleted, so that the second skeleton line is obtained.
For example, if a distance from a point on a certain side line on the first skeleton to the boundary of the unstructured road is smaller than a preset distance threshold value, the side line is eliminated.
Specifically, in one example, the distance from the path of the right-to-travel to the boundary of the unstructured road may be determined based on the width and the minimum turning radius of the unmanned vehicle, and thus the right-to-travel path may be determined based on the distance and the boundary of the unstructured road. Where the distance here may be the sum of half the width of the vehicle and the minimum turning radius.
In another example, a grid adjacent to an unvented region may be acquired; the right-to-travel path is determined based on the grid values of the grids adjacent to the non-trafficable region. For example, referring to fig. 3, if the grid value adjacent to the non-passable area is 1, the center point of the grid with the grid value of 1 is taken to be connected according to the grid value of 1, so as to determine the right-to-travel path.
In particular, a structured-like map, in which unstructured roads are processed to contain a plurality of edges and a plurality of vertices, is in fact a structured-like map.
Referring to fig. 6, fig. 6 is a schematic diagram of a second skeleton line and a right-to-travel path provided in an embodiment of the present invention, in which a solid line in fig. 6 is the second skeleton line, a dotted line is the right-to-travel path, and the second skeleton line and the right-to-travel path in fig. 6 are called side lines, an intersection point of each side line is called a vertex, and a side line is provided between two vertices, and the side line not only has a length attribute, but also has a direction attribute, the second skeleton line in the side line is two-way passable, and the right-to-travel path follows the principle of right-to-travel.
The following describes how to determine the directional properties of the edge:
specifically, firstly, any point on any side line on a right-to-travel path is obtained; then judging whether the first side point and the second side point of any point are in the unstructured road or not; the first side point and the second side point are arranged on two sides of any point; finally, when the first side point is not in the unstructured road, determining the direction attribute of any side line as a first direction, and when the second side point is not in the unstructured road, determining the direction attribute of any side line as a second direction; the first direction and the second direction are opposite to each other.
Referring to FIG. 7, taking the edge AB as an example, if the distance from AB to the boundary of the unstructured road is l, an initial direction of AB is given as indicated by the arrow in the figure, i.e., by APoint to B, take point C at any point on the AB edge, take point C along the direction normal to point C on the left and right sides of point C l And C r 。C l And C r The following formula is satisfied:
CC r ＝CC r =l+ε formula (1)
Wherein CC r For point C to point C r Length of line segment, CC l For point C to point C l Epsilon is a sufficiently small value that can be considered as infinitesimal.
By detecting C l And C r Whether within the unstructured road, the directional properties of the edge AB are determined. If C r If not in the unstructured road, the arrow in FIG. 8 is the direction of the AB edge, if C l If the road is not in the unstructured road, the opposite direction of the arrow in fig. 8 is the direction of the AB edge.
Referring to fig. 8, in a structured map like an unstructured road, a length attribute, a direction attribute, and a plurality of vertices of a side line are included. The direction attribute may be visually represented by an arrow and the length attribute may be stored in a database, e.g., an edge has an edge ID, the corresponding edge ID corresponding to the length attribute of the edge. The edge ID and length attributes may be stored in a database in tabular form. If the ID of the edge AB is 01 and the corresponding length is 5m, 01 and 5 meters are stored in the database in a tabular manner.
Specifically, the current position information of the vehicle can be obtained by sensors mounted on the vehicle, such as inertial measurement units (Inertial measurement unit, IMU), global positioning systems (Global Positioning System, GPS), etc. during the driving of the autonomous vehicle.
When the server receives the service information of the user terminal, for example, the user terminal receives the information of the departure place, the destination, the departure time and the like input by the user, and after receiving the information of the departure place and the destination sent by the user terminal, the server screens out the target vehicle and sends the information of the departure place, the destination, the departure time, the departure number and the like to the target vehicle.
For the target vehicle, when the vehicle is running, global path planning is needed to run from a departure place to a destination, and before the global path planning is performed, if the current position information and the destination, namely the target point, are both in the obtained class structured map with a plurality of side lines and a plurality of vertexes, the current position of the vehicle and the target point can be mapped in the class structured map.
The current position of the vehicle can be longitude and latitude data, each point in the similar structured map has position information, and the current longitude and latitude data of the vehicle can be mapped into the structured map, so that the current position of the vehicle and the target point in the structured map can be determined.
Specifically, step 160 includes: firstly, determining a starting circle by taking a first position as a circle center and taking a preset first radius as a radius; secondly, determining a first nearest point set in a starting circle, and taking each first nearest point in the first nearest point set as a starting point of global path planning; thirdly, determining an end point circle by taking the second position as a circle center and taking a preset second radius as a radius; and finally, determining a second nearest point set in the end point circle, and taking each second nearest point in the second nearest point set as an end point of the global path planning.
The preset first radius and the preset second radius may be the same or different. The preset first radius may be determined according to the width of the vehicle and a fixed value, where the fixed value is related to the number of edges around the first position and the distance between adjacent edges around the first position, for example, referring to fig. 9, the first position is a small rectangular frame below, and after the first position has three edges, the preset first radius is used to make a circle, so that all three edges are in a starting circle, and the three small five-pointed star around the small rectangular frame below the obtained first nearest point set is represented.
Accordingly, the setting of the end point circle is similar to the setting of the start circle, and will not be described here again. The second position is a small rectangular frame above, and the first position is provided with three side lines, and after the circle is made with a preset second radius, the obtained second nearest point set is represented by three small five-pointed stars around the small rectangular frame above.
Step 170 of determining an evaluation function value from each first closest point in the first closest point set to each second closest point in the second closest point set according to the length from each first closest point in the first closest point set to the sampling point along the edge, the direction from each first closest point to the sampling point along the edge, and the distance from the sampling point to each second closest point.
Specifically, step 170 includes: determining the length from each starting point to the sampling point along the edge line; determining the direction from each starting point to the sampling point along the side line; determining a first product of a preset first weight value and a length; determining a second product of a preset second weight value and a direction; determining a sum of the first product and the second product as a first generation value; determining the distance from the sampling point to the target point as a second cost value; and determining an evaluation function value according to the first generation value and the second generation value.
The sampling point is one point at each time of path search, and the path search can be implemented by using a path search algorithm. Such as the a-algorithm in the heuristic path search algorithm, etc., this application is not limited in this regard.
Specifically, in one example, when a heuristic path search algorithm is adopted, the evaluation may be performed by an evaluation function corresponding to the a-algorithm, where the evaluation function is shown in formula (2):
f=g+h formula (2)
The g value is the first generation value of the search and represents the cost of searching from the starting point of the search to the current sampling point. The value of h is the second cost value of the current sample point to the target point and may be expressed as the sample point to target point distance, where the distance includes, but is not limited to, euclidean distance and Manhattan distance.
Wherein the present application can calculate the first generation value by formula (3).
g=α×l+β×dir formula (3)
Wherein l represents the length from the starting point to the current sampling point along the edge line, the edge line is divided by the top points, the edge line between the two top points is used as an edge line, dir is the direction from the starting point to the sampling point along the edge line, the forward running is marked as 0, the backward running is marked as 1, alpha is a first weight coefficient, the importance of the path length to the whole path is represented, beta is a second weight coefficient, and the importance of the direction to the whole path is represented.
The values of the beta are smaller than the alpha value and the beta is infinitely smaller, and specific values of the beta are determined by multiple simulation calculation.
Subsequently, an evaluation function value can be determined according to the first-generation value and the second-generation value, and a corresponding optimal path with the minimum evaluation function value is determined according to the evaluation function value.
For example, the starting point is 1, 2 and 3, the sampling point is A, the end point is 4,5 and 6, the first generation value of three paths 1-A, 2-A and 3-A is calculated from the starting point to the sampling point, the second generation value of three paths A-4, A-5 and A-6 is calculated from the sampling point to the end point, then the second generation values of the three paths 1-A+A-4,1-A+A-5,1-A+A-6,2-A+A-4,2-A+A-5,2-A+A-6,3-A+A-4,3-A+A-5,3-A+A-6 are added at will, the 9 values are calculated, and the minimum evaluation function value is determined to be a local path, for example, the minimum value 1-A+A-4 is determined, and the starting point 1 to the sampling point A to the end point 4 are the optimal path.
By applying the global path planning method provided by the embodiment of the invention, the unstructured road can be subjected to topological treatment to obtain the structured map of the type, so that the efficiency of global path planning is improved through global path planning of the structured map of the type, and meanwhile, the length attribute and the direction attribute of the boundary line in the structured map of the type can be combined when path evaluation is carried out, so that the rationality of the path search result is improved.
Fig. 10 is a schematic structural diagram of a global path planning apparatus according to a second embodiment of the present invention, as shown in fig. 10, where the global path planning apparatus is applied to the global path planning method in the first embodiment, and the global path planning apparatus includes: an acquisition unit 210, a determination unit 220, and a mapping unit 230.
The acquiring unit 210 is configured to acquire a first skeleton line of a passable area in an unstructured road; the unstructured road comprises a passable area and an unvented area;
the determining unit 220 is configured to determine a second skeleton line according to a distance between each point in the first skeleton line and a boundary of the unstructured road;
the obtaining unit 210 is further configured to obtain a right-to-travel path of the passable area;
The determining unit 220 is further configured to determine a plurality of edges and a plurality of vertices of the unstructured road according to the second skeleton line and the right-to-travel path, where the plurality of edges and the plurality of vertices form a structured map; the side lines have length attributes and direction attributes, and the intersection point of any two side lines is the vertex of the unstructured road;
the mapping unit 230 is configured to map the current position and the target point of the vehicle onto the structured map before performing global path planning;
the determining unit 220 is further configured to determine a first closest point set from the first position to an edge of the class structured map and a second closest point set from the second position to an edge of the class structured map according to a first position of the current position of the vehicle in the class structured map and a second position of the target point in the class structured map;
the determining unit 220 is further configured to determine an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set according to a length from each first closest point in the first closest point set to the sampling point along the edge, a direction from each first closest point to the sampling point along the edge, and a distance from the sampling point to each second closest point;
The determining unit 220 is further configured to determine that a path between a first closest point to which the evaluation function value is smallest and a second closest point is an optimal path.
Further, the obtaining unit 210 is specifically configured to: rasterizing the passable area in the unstructured road to divide the passable area into a plurality of grids; determining a grid value of each grid according to the distance from each grid to the non-passable area; when any one of the grid values represents a different part of the non-passable area, determining the first skeleton line according to any one of the grid values.
Further, the obtaining unit 210 is specifically configured to: acquiring grids adjacent to the non-passable area; the right-to-travel path is determined based on the grid values of the grids adjacent to the non-trafficable region.
When the rasterization processing is carried out, determining the size of the grids according to the width of the vehicle; the distance from the right-to-travel path to the boundary of the unstructured road is determined based on the width of the vehicle and the minimum turning radius.
Further, the determining unit 220 is specifically configured to:
and deleting the points in the first skeleton line when the distance between the points in the first skeleton line and the boundary of the passable area is smaller than a preset distance threshold value, so as to obtain a second skeleton line.
The edge has a direction attribute, which specifically includes:
acquiring any point on any side line of a right-to-travel path;
judging whether the first side point and the second side point of any point are in the unstructured road or not; the first side point and the second side point are arranged on two sides of any point;
when the first side point is not in the unstructured road, determining the direction attribute of any side line as a first direction, and when the second side point is not in the unstructured road, determining the direction attribute of any side line as a second direction; the first direction and the second direction are opposite to each other.
Further, the determining unit 220 is specifically configured to:
determining a starting circle by taking the first position as a circle center and taking a preset first radius as a radius;
determining a first nearest point set in a starting circle, and taking each first nearest point in the first nearest point set as a starting point of global path planning;
determining a final point circle by taking the second position as a circle center and taking a preset second radius as a radius;
and determining a second closest point set in the end point circle, and taking each second closest point in the second closest point set as an end point of the global path planning.
Further, the determining unit 220 is specifically configured to:
determining the length from each starting point to the sampling point along the edge line; determining the direction from each starting point to the sampling point along the side line; determining a first product of a preset first weight value and a length; determining a second product of a preset second weight value and a direction; determining a sum of the first product and the second product as a first generation value; determining the distance from the sampling point to the target point as a second cost value; and determining an evaluation function value according to the first generation value and the second generation value.
By applying the global path planning device provided by the embodiment of the invention, the unstructured road can be subjected to topological treatment to obtain the structured map of the type, so that the efficiency of global path planning is improved through global path planning of the structured map of the type, and meanwhile, the length attribute and the direction attribute of the boundary line in the structured map of the type can be combined when path evaluation is carried out, so that the rationality of the path search result is improved.
An embodiment of the present invention provides an apparatus, including a memory and a processor, where the memory is configured to store a program, and the memory may be connected to the processor through a bus. The memory may be non-volatile memory, such as a hard disk drive and flash memory, in which software programs and device drivers are stored. The software program can execute various functions of the method provided by the embodiment of the invention; the device driver may be a network and interface driver. The processor is configured to execute a software program, where the software program is executed to implement the method provided in the first embodiment of the present invention.
A fourth embodiment of the present invention provides a computer program product containing instructions, which when executed on a computer, cause the computer to perform the method provided by the first embodiment of the present invention.
The fifth embodiment of the present invention provides a computer readable storage medium, where a computer program is stored, and when the computer program is executed by a processor, the method provided in the first embodiment of the present invention is implemented.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative elements and steps are described above generally in terms of function in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied in hardware, in a software module executed by a processor, or in a combination of the two. The software modules may be disposed in Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The foregoing detailed description of the invention has been presented for purposes of illustration and description, and it should be understood that the invention is not limited to the particular embodiments disclosed, but is intended to cover all modifications, equivalents, alternatives, and improvements within the spirit and principles of the invention.
Claims (10)
1. A global path planning method, characterized in that the global path planning method comprises:
acquiring a first skeleton line of a passable area in an unstructured road; the unstructured road comprises a passable area and an unvented area;
determining a second skeleton line according to the distance between each point in the first skeleton line and the boundary of the unstructured road;
acquiring a right-to-travel path of the passable area;
determining a plurality of side lines and a plurality of vertexes of the unstructured road according to the second skeleton line and the right-to-travel path, wherein the side lines and the vertexes form a similar structured map; the side lines have length attributes and direction attributes, and the intersection point of any two side lines is the vertex of the unstructured road;
Before global path planning is carried out, mapping the current position of the vehicle and a target point to the structured map;
determining a first nearest point set from the first position to an edge line of the similar structured map and a second nearest point set from the second position to the edge line of the similar structured map according to a first position of the current position of the vehicle in the similar structured map and a second position of the target point in the similar structured map;
determining an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set according to the length from each first closest point in the first closest point set to the sampling point along the edge line, the direction from each first closest point to the sampling point along the edge line and the distance from the sampling point to each second closest point;
and determining the path between the first nearest point with the minimum evaluation function value and the second nearest point as the optimal path.
2. The method according to claim 1, wherein the acquiring a first skeleton line of a passable area in an unstructured road comprises:
rasterizing a passable area in the unstructured road, and dividing the passable area into a plurality of grids;
Determining a grid value of each grid according to the distance from each grid to the non-passable area;
and when any grid value represents a different part from the non-passable area, determining a first skeleton line according to the any grid value.
3. The method of claim 2, wherein the acquiring the right-to-travel path of the passable zone comprises:
acquiring grids adjacent to the non-passable area;
and determining a right-to-travel path according to the grid value of the grids adjacent to the non-passable area.
4. The method according to claim 2, wherein the rasterizing process is performed by determining a grid size based on a width of the vehicle;
the distance from the right-to-travel path to the boundary of the unstructured road is determined based on the width of the vehicle and the minimum turning radius.
5. The method of claim 1, wherein determining a second skeleton line based on a distance of each point in the first skeleton line from a boundary of the unstructured road comprises:
and deleting the point in the first skeleton line to obtain a second skeleton line when the distance between the point in the first skeleton line and the boundary of the passable area is smaller than a preset distance threshold value.
6. The method according to claim 1, wherein the edge has a directional attribute comprising:
acquiring any point on any side line of a right-to-travel path;
judging whether the first side point and the second side point of any point are in the unstructured road or not; the first side point and the second side point are arranged on two sides of the arbitrary point;
when the first side point is not in the unstructured road, determining the direction attribute of any side line as a first direction, and when the second side point is not in the unstructured road, determining the direction attribute of any side line as a second direction; the first direction and the second direction are opposite to each other.
7. The method of claim 1, wherein the determining the first set of closest points on the first location to the edge of the structured map-like and the second set of closest points on the second location to the edge of the structured map-like comprises:
determining a starting circle by taking the first position as a circle center and taking a preset first radius as a radius;
determining a first nearest point set in the starting circle, and taking each first nearest point in the first nearest point set as a starting point of global path planning;
Determining an end point circle by taking the second position as a circle center and taking a preset second radius as a radius;
and determining a second closest point set in the end point circle, and taking each second closest point in the second closest point set as an end point of global path planning.
8. The method of claim 7, wherein determining the evaluation value of each first closest point in the first closest point set to each second closest point in the second closest point set based on the length of each first closest point in the first closest point set to the sampling point along the edge, the direction of each first closest point to the sampling point along the edge, and the distance of the sampling point to each second closest point comprises:
determining the length from each starting point to the sampling point along the edge line;
determining the direction from each starting point to the sampling point along the side line;
determining a first product of a preset first weight value and the length;
determining a second product of a preset second weight value and the direction;
determining a sum of the first product and the second product as a first generation value;
determining the distance from the sampling point to the target point as a second cost value;
and determining an evaluation function value according to the first generation value and the second generation value.
9. A global path planning apparatus, the global path planning apparatus comprising:
the acquisition unit is used for acquiring a first skeleton line of a passable area in the unstructured road; the unstructured road comprises a passable area and an unvented area;
a determining unit for determining a second skeleton line according to a distance of each point in the first skeleton line from a boundary of the unstructured road;
the acquisition unit is also used for acquiring a right-to-travel path of the passable area;
the determining unit is further configured to determine a plurality of edges and a plurality of vertices of the unstructured road according to the second skeleton line and the right-to-travel path, where the plurality of edges and the plurality of vertices form a similar structured map; the side lines have length attributes and direction attributes, and the intersection point of any two side lines is the vertex of the unstructured road;
the mapping unit is used for mapping the current position of the vehicle and the target point to the structured map before global path planning is carried out;
the determining unit is further configured to determine a first closest point set from a first position of a current position of the vehicle in the generic structured map and a second closest point set from the first position to an edge of the generic structured map, and from the second position to a second closest point set on the edge of the generic structured map, according to the first position of the current position of the vehicle in the generic structured map and the second position of the target point in the generic structured map;
The determining unit is further configured to determine an evaluation function value of each first closest point in the first closest point set to each second closest point in the second closest point set according to a length from each first closest point in the first closest point set to the sampling point along the edge line, a direction from each first closest point to the sampling point along the edge line, and a distance from the sampling point to each second closest point;
the determining unit is further configured to determine that a path between a first closest point to a second closest point where the evaluation function value is smallest is an optimal path.
10. A computer readable storage medium, characterized in that the computer readable storage medium has stored thereon a computer program which, when executed by a processor, performs the method according to any of claims 1-8.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011003654.1A CN112444263B (en) | 2020-09-22 | 2020-09-22 | Global path planning method and device |
| PCT/CN2021/118566 WO2022063005A1 (en) | 2020-09-22 | 2021-09-15 | Global path planning method and apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011003654.1A CN112444263B (en) | 2020-09-22 | 2020-09-22 | Global path planning method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112444263A CN112444263A (en) | 2021-03-05 |
| CN112444263B true CN112444263B (en) | 2023-05-23 |
Family
ID=74736831
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011003654.1A Active CN112444263B (en) | 2020-09-22 | 2020-09-22 | Global path planning method and device |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112444263B (en) |
| WO (1) | WO2022063005A1 (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112444263B (en) * | 2020-09-22 | 2023-05-23 | 北京智行者科技股份有限公司 | Global path planning method and device |
| CN113119995B (en) * | 2021-03-11 | 2023-01-31 | 京东鲲鹏(江苏)科技有限公司 | Path searching method and device, equipment and storage medium |
| CN113219973B (en) * | 2021-05-08 | 2022-06-24 | 浙江工业大学 | Local path control method of mobile robot |
| CN113928337B (en) * | 2021-09-30 | 2023-10-20 | 华为技术有限公司 | Method for guiding vehicle to run and related system and storage medium |
| CN116431964B (en) * | 2023-04-20 | 2024-04-19 | 浙江省水利河口研究院(浙江省海洋规划设计研究院) | Run-length stripping method for generating complex river network water system skeleton line |
| CN116543310B (en) * | 2023-06-30 | 2023-10-31 | 眉山环天智慧科技有限公司 | Road line extraction method based on Voronoi diagram and kernel density |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106092102A (en) * | 2016-07-20 | 2016-11-09 | 广州极飞电子科技有限公司 | A kind of unmanned plane paths planning method and device |
| CN109557928A (en) * | 2019-01-17 | 2019-04-02 | 湖北亿咖通科技有限公司 | Automatic driving vehicle paths planning method based on map vector and grating map |
| CN110823241A (en) * | 2019-11-19 | 2020-02-21 | 齐鲁工业大学 | Robot path planning method and system based on passable area skeleton extraction |
| CN110850871A (en) * | 2019-10-21 | 2020-02-28 | 深圳市银星智能科技股份有限公司 | Machine path planning method and mobile robot |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005321370A (en) * | 2004-04-05 | 2005-11-17 | Sony Corp | Navigation system, data processing method and computer program |
| US9524647B2 (en) * | 2015-01-19 | 2016-12-20 | The Aerospace Corporation | Autonomous Nap-Of-the-Earth (ANOE) flight path planning for manned and unmanned rotorcraft |
| CN107664503A (en) * | 2016-07-29 | 2018-02-06 | 上海汽车集团股份有限公司 | Vehicle path planning method and device |
| CN106767860B (en) * | 2016-11-21 | 2019-12-03 | 江苏大学 | A method of shortening intelligent automobile path planning search time based on heuristic search algorithm |
| CN107992050B (en) * | 2017-12-20 | 2021-05-11 | 广州汽车集团股份有限公司 | Method and device for planning local path motion of unmanned vehicle |
| CN111521189B (en) * | 2020-04-10 | 2022-02-15 | 北京智行者科技有限公司 | Cleaning path planning method and device |
| CN112444263B (en) * | 2020-09-22 | 2023-05-23 | 北京智行者科技股份有限公司 | Global path planning method and device |
-
2020
- 2020-09-22 CN CN202011003654.1A patent/CN112444263B/en active Active
-
2021
- 2021-09-15 WO PCT/CN2021/118566 patent/WO2022063005A1/en active Application Filing
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106092102A (en) * | 2016-07-20 | 2016-11-09 | 广州极飞电子科技有限公司 | A kind of unmanned plane paths planning method and device |
| CN109557928A (en) * | 2019-01-17 | 2019-04-02 | 湖北亿咖通科技有限公司 | Automatic driving vehicle paths planning method based on map vector and grating map |
| CN110850871A (en) * | 2019-10-21 | 2020-02-28 | 深圳市银星智能科技股份有限公司 | Machine path planning method and mobile robot |
| CN110823241A (en) * | 2019-11-19 | 2020-02-21 | 齐鲁工业大学 | Robot path planning method and system based on passable area skeleton extraction |
Non-Patent Citations (3)
| Title |
|---|
| Semantic Grid-Based Road Model Estimation for Autonomous Driving;Julian等;《2019 IEEE Intelligent Vehicles Symposium (IV)》;20190829;全文 * |
| 分类栅格图像序列渐变技术研究及应用;林恒;《中国博士学位论文全文数据库 基础科学辑 第09期》;20190915;全文 * |
| 地图目标群间骨架线提取的算法研究;刘远刚等;《武汉大学学报信息科学版》;20150228;第40卷(第2期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112444263A (en) | 2021-03-05 |
| WO2022063005A1 (en) | 2022-03-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112444263B (en) | Global path planning method and device | |
| CN110155053B (en) | Method and apparatus for providing information for driving a vehicle | |
| EP3109842B1 (en) | Map-centric map matching method and apparatus | |
| US11531110B2 (en) | LiDAR localization using 3D CNN network for solution inference in autonomous driving vehicles | |
| WO2020029601A1 (en) | Method and system for constructing transverse topological relationship of lanes in map, and memory | |
| US11594011B2 (en) | Deep learning-based feature extraction for LiDAR localization of autonomous driving vehicles | |
| Hashemi et al. | A critical review of real-time map-matching algorithms: Current issues and future directions | |
| JP6401140B2 (en) | Joint probability modeling and estimation of the structure of intersections | |
| JP7256758B2 (en) | LIDAR positioning with time smoothing using RNN and LSTM in autonomous vehicles | |
| CN102102992B (en) | Multistage network division-based preliminary screening method for matched roads and map matching system | |
| CN110531749A (en) | Determine the driving path for avoiding the automatic Pilot of moving obstacle | |
| US10818035B1 (en) | Representing vanishing points using relative distances | |
| CN109491378A (en) | The route guiding system based on roadway segment of automatic driving vehicle | |
| CN113924459B (en) | Dynamic sensor range detection for vehicle navigation | |
| US11120566B2 (en) | Determining vanishing points based on feature maps | |
| US11227167B2 (en) | Determining vanishing points based on lane lines | |
| EP4160146A1 (en) | Quadtree based data structure for storing information relating to an environment of an autonomous vehicle and methods of use thereof | |
| JP2023523350A (en) | Vehicle-based data processing method, data processing apparatus, computer apparatus, and computer program | |
| US20210381845A1 (en) | Determining landmark detectability | |
| CN113157842A (en) | Map generation method and device, electronic equipment and readable storage medium | |
| US20210270629A1 (en) | Method and apparatus for selecting a path to a destination | |
| CN114419573A (en) | Dynamic occupancy grid estimation method and device | |
| CN115230688B (en) | Obstacle trajectory prediction method, system, and computer-readable storage medium | |
| JP5679207B2 (en) | Own vehicle position recognition system, own vehicle position recognition program, and own vehicle position recognition method | |
| KR101622176B1 (en) | Method generating terrain data for global path planning |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20210806 Address after: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Applicant after: Beijing Idriverplus Technology Co.,Ltd. Address before: 401122 No.1, 1st floor, building 3, No.21 Yunzhu Road, Yubei District, Chongqing Applicant before: Chongqing Zhixing Information Technology Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| CB02 | Change of applicant information |
Address after: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Applicant after: Beijing Idriverplus Technology Co.,Ltd. Address before: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096 Applicant before: Beijing Idriverplus Technology Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |