CN116380082A - Quick path planning method and system for unmanned vehicle in unknown environment - Google Patents

Quick path planning method and system for unmanned vehicle in unknown environment Download PDF

Info

Publication number
CN116380082A
CN116380082A CN202310664164.3A CN202310664164A CN116380082A CN 116380082 A CN116380082 A CN 116380082A CN 202310664164 A CN202310664164 A CN 202310664164A CN 116380082 A CN116380082 A CN 116380082A
Authority
CN
China
Prior art keywords
polygon
filtering
obstacle
unmanned vehicle
path planning
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.)
Granted
Application number
CN202310664164.3A
Other languages
Chinese (zh)
Other versions
CN116380082B (en
Inventor
张旭东
范晓临
邹渊
樊杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202310664164.3A priority Critical patent/CN116380082B/en
Publication of CN116380082A publication Critical patent/CN116380082A/en
Application granted granted Critical
Publication of CN116380082B publication Critical patent/CN116380082B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/20—Instruments for performing navigational calculations
    • 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/3407—Route searching; Route guidance specially adapted for specific applications
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a rapid path planning method and a rapid path planning system for an unmanned vehicle in an unknown environment, and relates to the field of unmanned vehicles, wherein the method comprises the steps of projecting three-dimensional point cloud data of a target environment to a binarization plane taking the ground as a reference, and determining an obstacle polygon in the target environment according to edge point data on the binarization plane; filtering the obstacle polygons by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method; and determining the optimal passing path of the target environment according to the filtered obstacle polygons. The invention simplifies the obstacle polygon by adopting the first polygon filtering method, the second polygon filtering method and the third polygon filtering method, reduces the calculated amount for constructing the image and planning the path of the environment containing a plurality of obstacle polygons, and improves the speed of path planning.

Description

Quick path planning method and system for unmanned vehicle in unknown environment
Technical Field
The invention relates to the field of unmanned vehicles, in particular to a rapid path planning method and system for an unmanned vehicle in an unknown environment.
Background
The path planning is to plan the motion track of the unmanned vehicle by a reasonable method under the condition that the map and the current state of the unmanned vehicle are known, so that the unmanned vehicle can reach a destination while avoiding an obstacle, which is one of the essential important problems in the technical field of unmanned vehicles.
However, when the unmanned vehicle needs to perform route planning at a place where a complex obstacle exists, the factors of the shape, structure and the like of the object with the complex obstacle can cause the calculation cost of the unmanned vehicle route planning to increase exponentially, which greatly reduces the route planning efficiency of the unmanned vehicle.
Disclosure of Invention
The invention aims to provide a rapid path planning method and system for an unmanned vehicle in an unknown environment, wherein the path planning efficiency is high.
In order to achieve the above object, the present invention provides the following solutions:
the invention provides a rapid path planning method of an unmanned vehicle in an unknown environment, which comprises the following steps:
step 1: projecting three-dimensional point cloud data of a target environment to a ground-based binarization plane, and determining an obstacle polygon in the target environment according to edge points on the projected binarization plane;
step 2: filtering the vertexes of the obstacle polygon by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method;
the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide;
the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set according to the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon;
the third polygon filtering method comprises the steps of judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than a set angle; if yes, filtering the vertex between the two adjacent edges;
step 3: and determining the optimal passing path of the target environment according to the obstacle polygons formed by the filtered vertexes.
Optionally, before step 1, the method further includes:
and collecting three-dimensional point cloud data of the target environment by adopting a laser radar.
Optionally, the step 1 specifically includes:
performing downsampling processing on the three-dimensional point cloud data based on a PCL method;
projecting the down-sampled three-dimensional point cloud data to a ground-based binarization plane, and determining a binarization image of the target environment;
filtering the binarized image based on a balance filter;
and determining an obstacle polygon in the target environment according to the edge points in the filtered binarized image.
Optionally, after the step 1, before the step 2, the method further includes:
judging whether the obstacle polygon carries out polygon filtering or not according to the number of the vertexes of the obstacle polygon, and specifically comprising the following steps:
when the number of the vertexes of the obstacle polygon is more than 10, performing polygon filtering on the obstacle polygon;
and when the number of the vertexes of the obstacle polygon is smaller than 10, performing no polygon filtering on the obstacle polygon.
Optionally, the step 3 specifically includes:
determining a visual view of the target environment according to the filtered obstacle polygon;
from the visual view, an optimal path of the target environment is determined based on a modified a-algorithm.
Optionally, the modified a-algorithm specifically includes the following steps:
F(x)=G(x)+H(x)+T(x)；
wherein T (x) is a transit time heuristic, G (x) is a path length heuristic from the starting node to the current node, and H (x) is an estimated cost heuristic from the current node to the target node.
Optionally, the passing time heuristic is specifically as follows:
Figure SMS_1
；
wherein ,
Figure SMS_2
for the weight coefficient in the cost function, +.>
Figure SMS_3
The heading angle change value of the unmanned vehicle at the node is obtained.
The invention also provides a rapid path planning system of the unmanned vehicle in an unknown environment, which comprises the following steps:
the data processing module is used for projecting three-dimensional point cloud data of the target environment to a binarization plane taking the ground as a reference, and determining an obstacle polygon in the target environment according to edge point data on the binarization plane;
the filtering module is used for filtering the obstacle polygons by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method; the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide; the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set according to the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon;
and the path planning module is used for determining the optimal passing path of the target environment according to the filtered obstacle polygon.
Optionally, the data processing module includes:
the downsampling unit is used for downsampling the three-dimensional point cloud data based on a PCL method;
the image unit is used for projecting the three-dimensional point cloud data after the downsampling to a binarization plane taking the ground as a reference, and determining a binarization image of the target environment;
a filtering unit for filtering the binarized image based on a balance filter;
and the drawing unit is used for determining an obstacle polygon in the target environment according to the edge points in the filtered binarized image.
Optionally, the path planning module includes:
a visual view unit, configured to determine a visual view of the target environment according to the filtered obstacle polygon;
and the path planning unit is used for determining the optimal passing path of the target environment based on an improved A-x algorithm according to the visual view.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
the invention provides a rapid path planning method and a rapid path planning system for an unmanned vehicle in an unknown environment, wherein the method comprises the following steps: projecting three-dimensional point cloud data of a target environment to a binarization plane taking the ground as a reference, and determining an obstacle polygon in the target environment according to edge point data on the binarization plane; filtering the obstacle polygons by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method; the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide; the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length; if yes, filtering the vertex between two adjacent edges; the second set length is set according to the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon; and determining the optimal passing path of the target environment according to the filtered obstacle polygons. The invention simplifies the obstacle polygon by adopting the first polygon filtering method, the second polygon filtering method and the third polygon filtering method, reduces the calculated amount of path planning for the environment containing complex obstacles, and improves the path planning efficiency.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions of the prior art, the drawings that are needed in the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flowchart of a fast path planning method of an unmanned vehicle in an unknown environment according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a first polygon filtering method according to an embodiment of the present invention; fig. 2 (a) is a pre-filtering obstacle polygon, and fig. 2 (b) is a post-filtering obstacle polygon;
FIG. 3 is a schematic diagram of a second polygon filtering method according to an embodiment of the present invention; fig. 3 (a) is a pre-filtering obstacle polygon, and fig. 3 (b) is a post-filtering obstacle polygon;
FIG. 4 is a schematic diagram of a third polygon filtering method according to an embodiment of the present invention; fig. 4 (a) is a pre-filtering obstacle polygon, and fig. 4 (b) is a post-filtering obstacle polygon;
fig. 5 is a schematic diagram of a path planning principle according to an embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention aims to provide a rapid path planning method and a rapid path planning system for an unmanned vehicle in an unknown environment, which can reduce the calculated amount of drawing and path planning and improve the speed of path planning.
In order that the above-recited objects, features and advantages of the present invention will become more readily apparent, a more particular description of the invention will be rendered by reference to the appended drawings and appended detailed description.
As shown in fig. 1, the present invention provides a method for fast path planning of an unmanned vehicle in an unknown environment, comprising:
step 1: projecting three-dimensional point cloud data of a target environment to a ground-based binarization plane, and determining an obstacle polygon in the target environment according to edge points on the projected binarization plane.
Step 2: and filtering the vertexes of the obstacle polygon by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method.
The first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide.
The second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set with the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon.
The third polygon filtering method comprises the steps of judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than a set angle; and if so, filtering the vertex between the two adjacent edges.
Step 3: and determining the optimal passing path of the target environment according to the obstacle polygons formed by the filtered vertexes.
In some embodiments, in step 1, it may further include:
and collecting three-dimensional point cloud data of the target environment based on the laser radar.
Specifically, the laser radar collects point cloud data S, a point cloud map is established through a slam method, and then the point cloud is downsampled through a PCL method to obtain a downsampled point cloud map S'.
In some embodiments, the step 1 may specifically include:
and performing downsampling processing on the three-dimensional point cloud data based on a PCL method.
And projecting the down-sampled three-dimensional point cloud data to a ground-based binarization plane, and determining a binarization image of the target environment.
The binarized image is filtered based on a balance filter.
And determining an obstacle polygon in the target environment according to the edge points in the filtered binarized image.
The down-sampling processing is performed on the three-dimensional point cloud data based on the PCL method, which can be specifically as follows:
the invention can perform downsampling processing on the three-dimensional point cloud data based on a pixel Grid downsampling method in the PCL method. An object of Voxel Grid filter is first defined. A filtering Cloud (point Cloud data that needs to be downsampled) is defined. Raw PointCloud data (three-dimensional point cloud data) is input into Voxel Grid filter. The PointCloud data is downsampled. And outputting the point cloud processed by the filter.
Specifically, downsampling three-dimensional point cloud data using the PCL method may bring several advantages:
1) Reducing the data volume: the point cloud data is often very bulky, and downsampling it can reduce the amount of data, making it easier to store, transmit, and process.
2) Noise removal: in acquiring, reconstructing, or processing point cloud data, noise or outliers often exist that can interfere with subsequent processing and analysis. By downsampling the point cloud data, these noise points can be removed, making the data cleaner and more accurate.
3) The treatment efficiency is improved: downsampling may result in a reduced point cloud density, thereby reducing the time and computing resources required for subsequent processing. Therefore, in the processing of large-scale point cloud data, downsampling is an effective data preprocessing means.
4) The storage space is significantly released: the data volume is reduced, so that the storage burden of unnecessary data is effectively reduced.
The PCL method is used for downsampling the point cloud, so that the processing efficiency of the point cloud data can be greatly improved, the consumption of storage and calculation resources is reduced, noise points are effectively removed, and the accuracy of the data is improved. These factors may make the point cloud data better applicable to unmanned, autopilot, robotic, etc. directions.
The filtering of the binarized image based on the balance filter may specifically be as follows:
the balance filter reduces the effects of noise in the image by averaging the color values around each pixel in the binarized image. Such filters typically use a convolution kernel of size n x n, where n is an odd number, to ensure that the center of the kernel can be located to the current pixel. The convolution operation is implemented by weighted averaging the surrounding n x n pixels of each pixel. The averaging filter may blur some details in the image while removing noise. Finally, according to the color gray value of each pixel point, a gray image I can be obtained 2 。
Wherein, according to the edge point in the filtered binarized image, determining an obstacle polygon in the target environment may specifically be as follows:
extracting gray scale image I 2 Edge points in the rule, and analyzing the topological structure of the edge points to obtain the first
Figure SMS_4
Vertices of a polygon of a barrier +.>
Figure SMS_5
。
In some embodiments, before step 2, it may further include:
and judging whether the obstacle polygon carries out polygon filtering or not according to the number of the vertexes of the obstacle polygon.
Specifically, when the number of vertices of the obstacle polygon is greater than 10, polygon filtering is performed on the obstacle polygon. And when the number of the vertexes of the obstacle polygon is smaller than 10, performing no polygon filtering on the obstacle polygon. In the implementation process, the number of the vertexes can be set according to actual situations. For example, the number of vertices of the obstacle polygon in the target environment is less than 10, then the threshold may be adjusted appropriately.
In this embodiment, the step 2 may specifically include:
and filtering the obstacle polygon by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method.
First calculate a point
Figure SMS_6
Whether an outer or an inner chamfer.
At this time, if the vertices of the polygon are arranged and stored in a counterclockwise order, then for the first
Figure SMS_7
，/>
Figure SMS_8
，/>
Figure SMS_9
The vertex is first calculated +.>
Figure SMS_10
Whether the positions of the vertices are concave angles or not, defined by a vector cross product, includes:
Figure SMS_11
。
where x represents vector cross-over; * Representing a number product; x is x i ，y i Respectively represent
Figure SMS_12
The x-and y-coordinates of the point.
If the result is greater than 0, then
Figure SMS_13
At->
Figure SMS_14
Clockwise, i.e. point->
Figure SMS_15
The corresponding angle is an outer angle; if the result of the above formula is less than 0, then +.>
Figure SMS_16
At->
Figure SMS_17
Counterclockwise, i.e. dot->
Figure SMS_18
The corresponding angle is a concave angle.
If it is
Figure SMS_19
The position is a concave angle, so that no collision risk exists after filtering, and the following steps are continued. If you get->
Figure SMS_20
The part is an outer convex corner, and then is filteredThere is a risk of collision afterwards, skip this +.>
Figure SMS_21
。
Specifically, the first polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; and if so, filtering the vertex between the two adjacent edges.
Specifically, the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; and if so, filtering the vertex between the two adjacent edges.
Specifically, the third polygon filtering method includes judging whether a concave angle formed by any two adjacent sides in the obstacle polygon is smaller than a set angle; and if so, filtering the vertex between the two adjacent edges.
The first polygonal filtering method is to filter with an unmanned vehicle model, and specifically can be as follows:
the vehicle occupies a circle in the map, namely, the circumcircle of the vehicle takes the geometric center of the vehicle as the center of a circle, takes the distance without collision as the radius, and takes the diameter of the circle as d collision (i.e., the first set length).
Calculating the Euclidean distance between each vertex of the obstacle polygon, for the first
Figure SMS_22
Vertex and->
Figure SMS_23
Distance between vertices->
Figure SMS_24
The formula is as follows:
Figure SMS_25
。
wherein ,xi ，y i Respectively represent
Figure SMS_26
The x-and y-coordinates of the point.
As shown in FIG. 2 (a) and FIG. 2 (b), for the first
Figure SMS_27
The distance from the ith vertex to the (i-1) th vertex is d i-1 And (d)
Figure SMS_28
The distance between the vertexes is->
Figure SMS_29
. When condition d is satisfied i-1 <dcollision and d i <dcollision, will be->
Figure SMS_30
Edges associated with the vertices and the ith vertex are filtered, and the index of points after the ith point is automatically decremented by 1.
After the filtering of the first polygon filtering method is completed, the vertexes of the obstacle polygons are obtained
Figure SMS_31
。
The second polygonal filtering method continues filtering according to the size of the obstacle, and specifically can be as follows:
as a filtering threshold, 0.05 times the diagonal length of the smallest circumscribed rectangle that can include a polygon is taken.
Obtaining the maximum value x of the horizontal coordinate in the vertex through traversing max And a minimum value x min Obtaining the span of the polygon in the transverse direction:
x contour =x max -x min 。
the same method results in a span of polygons in the longitudinal direction:
y contour =y max -y min 。
then set up
Figure SMS_32
For the "scale" of the polygon, the calculation formula is as follows:
Figure SMS_33
。
as shown in fig. 3 (a) and 3 (b), for the first
Figure SMS_34
For the vertices, if and->
Figure SMS_35
Distance of the vertices>
Figure SMS_36
And (4) and->
Figure SMS_37
Distance of the vertices>
Figure SMS_38
When the condition is satisfied: />
Figure SMS_39
When it is, will be->
Figure SMS_40
Edges associated with the ith vertex will be filtered, and the labels of vertices following the ith vertex are automatically decremented by 1.
Specifically, the ratio can be 0.05, and can be adjusted according to the actual environment and experimental conditions.
The third polygon filtering method filters the obstacle polygons according to the inner angle size, which may specifically be as follows:
as shown in fig. 4 (a) and 4 (b), in the polygon, there are some smoother partial contours that will have a sharp inward concave angle, at which point the vertices with too small a concave angle may be deleted.
Then for the ith i-1, i+1 vertices, calculate the ith
Figure SMS_41
Cosine values of the concave angle at each vertex. According to the vector dot product definition: />
Figure SMS_42
。
wherein ,
Figure SMS_43
representing vector dot product; * Representing a number product; />
Figure SMS_44
Respectively indicate->
Figure SMS_45
The x-and y-coordinates of the point.
Then calculating the angle of the angle at the moment according to a triangle formula to set a threshold angle max Taking this value as 30 degrees for the maximum value of the concave angle, if the condition cos (angle) is satisfied>cos(angle max ) Description of too small a concave angle, deletion and the first
Figure SMS_46
Side associated with each vertex>
Figure SMS_47
The labels of the points after the individual points are automatically decremented by 1. After this filtering is completed, the vertices of the polygon are obtained
Figure SMS_48
。
Specifically, the maximum value of the inner concave angle is determined according to the practical application environment, and the maximum value of the angle is an angle at which the unmanned vehicle cannot enter the inner concave angle part.
As described above, the purpose of filtering vertices in an obstacle polygon is to reduce the amount of computation by simplifying the polygon. The first set length is set according to the size of the unmanned vehicle, and the too short side of the obstacle polygon is removed. The second set length is filtered according to the size of the obstacle polygon, and when the obstacle is large, the accuracy requirement on the obstacle polygon is properly reduced, so that the polygon is simplified. Where ratio is 0.05 obtained by experiment, and may be modified according to the specific experimental environment in practical application.
In some embodiments, the step 3 may specifically include:
and determining a visual view of the target environment according to the filtered obstacle polygon.
From the visual view, an optimal path of the target environment is determined based on a modified a-algorithm.
And determining a visual view of the target environment according to the filtered obstacle polygon, wherein the visual view is specifically as follows:
by the outline characteristic points of the filtered obstacle
Figure SMS_49
A visual view is constructed.
In the local map, the outline points of the obstacle are taken as vertexes in the visual view, and the adjacent vertexes of the same obstacle form sides of the obstacle in the visual view.
For a polygonal map, there are a local map and a global map, and for a vertex in the local map and a vertex in the global map, if the euclidean distance of the two vertices is smaller than a certain threshold, it is assumed that the two vertices are actually represented at the same coordinate point, and they are associated. After the local map is obtained through the previous steps, the global map is updated once every time.
Wherein, according to the visual view, based on an improved a algorithm, an optimal passing path of the target environment is determined, which can be specifically as follows:
planning navigation is based on visual view using a modified a-algorithm. In consideration of the speed and safety of navigation, heuristic items based on the passing time and safety are added into the algorithm. Based on the time required by the vehicle to traverse the arc there, a time-based heuristic is generated
Figure SMS_50
. Generating a security-based heuristic according to the change of the course angle of the path at the node>
Figure SMS_51
. The heuristic at each node at this time is: />
Figure SMS_52
； wherein />
Figure SMS_53
The heuristic is a heuristic in a heuristic function of a traditional A-algorithm.
Taking fig. 5 as an example, when the planning program is run to
Figure SMS_55
At the point, add +.>
Figure SMS_60
，
Figure SMS_63
, in the formula ,/>
Figure SMS_57
As weight coefficients in the cost function. />
Figure SMS_58
Is the angle between the front and rear paths of the vertex, namely the course angle change value of the vehicle at the vertex. For example angle->
Figure SMS_61
Is->
Figure SMS_64
，/>
Figure SMS_54
The change of the course angle of the vehicle is minimum, the smoothness of the running path of the vehicle is highest, the required steering time when passing through the vertex is also minimum, < >>
Figure SMS_59
The value of (2) is minimal. The three paths to be selected for blue in the figure are only for +.>
Figure SMS_62
For the sake of +>
Figure SMS_65
The smaller the value of the heuristic function obtained by the path, +.>
Figure SMS_56
The greater the value of the heuristic function that the path gets.
In summary, when the course angle change of the selected path at the vertex is smaller, the smoothness of the path of the vehicle is higher, the time required for the vehicle to pass through the position is less, meanwhile, the safety is higher, the rapid and safe passing of the vehicle is facilitated, and the heuristic function value is smaller. Planning is carried out according to the heuristic function, so that an optimal path which can ensure the navigation speed and the safety at the same time is obtained.
The invention also provides a rapid path planning system of the unmanned vehicle in an unknown environment, which comprises the following steps:
the data processing module is used for projecting the three-dimensional point cloud data of the target environment to a binarization plane taking the ground as a reference, and determining an obstacle polygon in the target environment according to the edge point data on the binarization plane.
The filtering module is used for filtering the obstacle polygons by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method; the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide; the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set with the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon.
And the path planning module is used for determining the optimal passing path of the target environment according to the filtered obstacle polygon.
In some embodiments, the data processing module may include:
and the downsampling unit is used for downsampling the three-dimensional point cloud data based on a PCL method.
And the image unit is used for projecting the three-dimensional point cloud data after the downsampling to a binarization plane taking the ground as a reference to determine a binarization image of the target environment.
And the filtering unit is used for filtering the binarized image based on a balance filter.
And the drawing unit is used for determining an obstacle polygon in the target environment according to the edge points in the filtered binarized image.
In some embodiments, the path planning module may include:
and the visual view unit is used for determining the visual view of the target environment according to the filtered obstacle polygon.
And the path planning unit is used for determining the optimal passing path of the target environment based on an improved A-x algorithm according to the visual view.
In summary, the invention has the following advantages:
(1) The method filters the polygons based on the vehicle model and the obstacle size, and adds the filtering method of the concave angle vertexes, so that the polygons in the map are simplified, the calculation amount of subsequent map building and planning is reduced, and the speed of path planning is improved.
(2) The invention provides an improved A-algorithm based on a visual image, which is based on a visual image building and a traditional A-algorithm, and can add a heuristic item based on the passing time and safety on the basis of shortest distance to obtain an optimal path, thereby reducing the navigation time and ensuring the safety of a vehicle in the running process.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to assist in understanding the methods of the present invention and the core ideas thereof; also, it is within the scope of the present invention to be modified by those of ordinary skill in the art in light of the present teachings. In view of the foregoing, this description should not be construed as limiting the invention.

Claims (10)

1. A method for rapid path planning for an unmanned vehicle in an unknown environment, comprising:
step 1: projecting three-dimensional point cloud data of a target environment to a ground-based binarization plane, and determining an obstacle polygon in the target environment according to edge points on the projected binarization plane;
step 2: filtering the vertexes of the obstacle polygon by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method;
the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set according to the diameter of the minimum circumscribed circle of the unmanned vehicle;
the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set according to the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon;
the third polygon filtering method comprises the steps of judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than a set angle; if yes, filtering the vertex between the two adjacent edges;
step 3: and determining the optimal passing path of the target environment according to the obstacle polygons formed by the filtered vertexes.
2. The method for rapid path planning for an unmanned vehicle in an unknown environment of claim 1, further comprising, prior to step 1:
and collecting three-dimensional point cloud data of the target environment by adopting a laser radar.
3. The method for rapid path planning of an unmanned vehicle in an unknown environment according to claim 1, wherein step 1 specifically comprises:
performing downsampling processing on the three-dimensional point cloud data based on a PCL method;
projecting the down-sampled three-dimensional point cloud data to a ground-based binarization plane, and determining a binarization image of the target environment;
filtering the binarized image based on a balance filter;
and determining an obstacle polygon in the target environment according to the edge points in the filtered binarized image.
4. The method for rapid path planning for an unmanned vehicle in an unknown environment according to claim 1, further comprising, after step 1, before step 2:
judging whether the obstacle polygon carries out polygon filtering or not according to the number of the vertexes of the obstacle polygon, and specifically comprising the following steps:
when the number of the vertexes of the obstacle polygon is more than 10, performing polygon filtering on the obstacle polygon;
and when the number of the vertexes of the obstacle polygon is smaller than 10, performing no polygon filtering on the obstacle polygon.
5. The method for rapid path planning of an unmanned vehicle in an unknown environment according to claim 1, wherein the step 3 specifically comprises:
determining a visual view of the target environment according to the filtered obstacle polygons;
from the visual view, an optimal path of the target environment is determined based on a modified a-algorithm.
6. The method for fast path planning for an unmanned vehicle in an unknown environment according to claim 5, wherein the modified a-algorithm is specifically as follows:
F(x)=G(x)+H(x)+T(x)；
wherein T (x) is a transit time heuristic, G (x) is a path length heuristic from the starting node to the current node, and H (x) is an estimated cost heuristic from the current node to the target node.
7. The method for rapid path planning of an unmanned vehicle in an unknown environment according to claim 6, wherein the transit time heuristic is specifically as follows:
Figure QLYQS_1
；
wherein ,
Figure QLYQS_2
for the weight coefficient in the cost function, +.>
Figure QLYQS_3
The heading angle change value of the unmanned vehicle at the node is obtained.
8. A rapid path planning system for an unmanned vehicle in an unknown environment, comprising:
the data processing module is used for projecting three-dimensional point cloud data of the target environment to a binarization plane taking the ground as a reference, and determining an obstacle polygon in the target environment according to edge point data on the binarization plane;
the filtering module is used for filtering the obstacle polygons by adopting a first polygon filtering method, a second polygon filtering method and a third polygon filtering method; the first polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a first set length or not; if yes, filtering the vertex between the two adjacent edges; the first set length is set with the minimum diameter of the unmanned vehicle which does not collide; the second polygonal filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are smaller than a second set length or not; if yes, filtering the vertex between the two adjacent edges; the second set length is set according to the set proportional length of the longest diagonal line in the minimum circumscribed quadrangle of the obstacle polygon;
and the path planning module is used for determining the optimal passing path of the target environment according to the filtered obstacle polygon.
9. The rapid path planning system for an unmanned vehicle in an unknown environment of claim 8, wherein the data processing module comprises:
the downsampling unit is used for downsampling the three-dimensional point cloud data based on a PCL method;
the image unit is used for projecting the down-sampled three-dimensional point cloud data to a binarization plane taking the ground as a reference, and determining a binarization image of the target environment;
a filtering unit for filtering the binarized image based on a balance filter;
and the drawing unit is used for determining the obstacle polygon in the target environment according to the edge points in the filtered binarized image.
10. The rapid path planning system for an unmanned vehicle in an unknown environment of claim 8, wherein the path planning module comprises:
a visual view unit, configured to determine a visual view of the target environment according to the filtered obstacle polygon;
and the path planning unit is used for determining the optimal passing path of the target environment based on an improved A-x algorithm according to the visual view.
CN202310664164.3A 2023-06-07 2023-06-07 Quick path planning method and system for unmanned vehicle in unknown environment Active CN116380082B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310664164.3A CN116380082B (en) 2023-06-07 2023-06-07 Quick path planning method and system for unmanned vehicle in unknown environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310664164.3A CN116380082B (en) 2023-06-07 2023-06-07 Quick path planning method and system for unmanned vehicle in unknown environment

Publications (2)

Publication Number Publication Date
CN116380082A true CN116380082A (en) 2023-07-04
CN116380082B CN116380082B (en) 2023-08-08

Family

ID=86966075

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310664164.3A Active CN116380082B (en) 2023-06-07 2023-06-07 Quick path planning method and system for unmanned vehicle in unknown environment

Country Status (1)

Country Link
CN (1) CN116380082B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118466517A (en) * 2024-07-09 2024-08-09 南京师范大学 Robot path planning method based on visibility graph construction

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100289801A1 (en) * 2009-05-15 2010-11-18 Microsoft Corporation Interactive connector routing between obstacles
WO2018010471A1 (en) * 2016-07-12 2018-01-18 中国能源建设集团广东省电力设计研究院有限公司 Method and system for optimizing obstacle avoidance path of offshore wind farm current collection system
JP2018041244A (en) * 2016-09-07 2018-03-15 ボッシュ株式会社 Processing device and processing method for generating information on obstacle around mobile body
CN111562787A (en) * 2020-05-28 2020-08-21 长沙中联重科环境产业有限公司 Robot full-coverage path planning region dividing method, device, medium and equipment
CN112270642A (en) * 2020-10-30 2021-01-26 深兰人工智能(深圳)有限公司 Method, system and device for constructing external rectangular frame of obstacle point cloud
CN115359083A (en) * 2022-08-18 2022-11-18 北京纵目安驰智能科技有限公司 Method, system, medium, and electronic device for obtaining contour of obstacle

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100289801A1 (en) * 2009-05-15 2010-11-18 Microsoft Corporation Interactive connector routing between obstacles
WO2018010471A1 (en) * 2016-07-12 2018-01-18 中国能源建设集团广东省电力设计研究院有限公司 Method and system for optimizing obstacle avoidance path of offshore wind farm current collection system
JP2018041244A (en) * 2016-09-07 2018-03-15 ボッシュ株式会社 Processing device and processing method for generating information on obstacle around mobile body
CN111562787A (en) * 2020-05-28 2020-08-21 长沙中联重科环境产业有限公司 Robot full-coverage path planning region dividing method, device, medium and equipment
CN112270642A (en) * 2020-10-30 2021-01-26 深兰人工智能(深圳)有限公司 Method, system and device for constructing external rectangular frame of obstacle point cloud
CN115359083A (en) * 2022-08-18 2022-11-18 北京纵目安驰智能科技有限公司 Method, system, medium, and electronic device for obtaining contour of obstacle

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
DANNY Z. CHEN, 等: "Computing Shortest Paths among Curved Obstacles in the Plane", ACM TRANSACTIONS ON ALGORITHMS, vol. 11, no. 4, XP058067856, DOI: 10.1145/2660771 *
黄小毛;张垒;TANG LIE;唐灿;李小霞;贺小伟;: "复杂边界田块旋翼无人机自主作业路径规划", 农业机械学报, no. 03 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118466517A (en) * 2024-07-09 2024-08-09 南京师范大学 Robot path planning method based on visibility graph construction
CN118466517B (en) * 2024-07-09 2024-10-15 南京师范大学 Robot path planning method based on visibility graph construction

Also Published As

Publication number Publication date
CN116380082B (en) 2023-08-08

Similar Documents

Publication Publication Date Title
CN107702716B (en) Unmanned driving path planning method, system and device
Danescu et al. Modeling and tracking the driving environment with a particle-based occupancy grid
CN116380082B (en) Quick path planning method and system for unmanned vehicle in unknown environment
Manz et al. Monocular model-based 3D vehicle tracking for autonomous vehicles in unstructured environment
CN110826386A (en) LIDAR-based object detection and classification
CN114111825B (en) Path planning method, path planning device, electronic equipment, engineering machinery and storage medium
CN115639823B (en) Method and system for controlling sensing and movement of robot under rugged undulating terrain
Saleem et al. Steering angle prediction techniques for autonomous ground vehicles: a review
CN116740667B (en) Intersection surface data generation method and device, electronic equipment and storage medium
Wang et al. Map-enhanced ego-lane detection in the missing feature scenarios
WO2023092870A1 (en) Method and system for detecting retaining wall suitable for automatic driving vehicle
CN111488783A (en) Method and device for detecting pseudo-3D bounding box based on CNN
CN113635898A (en) Method, apparatus, storage medium, and program product for vehicle speed limit control
Xu et al. Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs
CN115330969A (en) Local static environment vectorization description method for ground unmanned vehicle
CN117419738A (en) Based on visual view and D * Path planning method and system of Lite algorithm
CN113189610B (en) Map-enhanced autopilot multi-target tracking method and related equipment
Makarov et al. Smoothing Voronoi-Based Path with Minimized Length and Visibility Using Composite Bezier Curves.
CN116430906A (en) Unmanned aerial vehicle dynamic obstacle avoidance method, system, equipment and medium based on bump translation
CN115937825A (en) Robust lane line generation method and device under BEV (beam-based attitude vector) of on-line pitch angle estimation
Mentasti et al. Two algorithms for vehicular obstacle detection in sparse pointcloud
CN110889362A (en) Obstacle detection method using grid map height information
CN115657675A (en) Vehicle motion path generation method and system and storage medium
Keen et al. Generation of elevation maps for planning and navigation of vehicles in rough natural terrain
CN113226885A (en) Method and device for determining steering intention of target vehicle

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
GR01 Patent grant
GR01 Patent grant