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

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

A fast path planning method and system for an unmanned vehicle in an unknown environment

technical field

The invention relates to the field of unmanned vehicles, in particular to a fast path planning method and system for an unmanned vehicle in an unknown environment.

Background technique

Path planning refers to planning the trajectory of the unmanned vehicle in a reasonable way when the map and the current state of the unmanned vehicle are known, so that the unmanned vehicle can reach the destination while avoiding obstacles. One of the essential and important issues in the field of vehicle technology.

However, when unmanned vehicles need to perform route planning in places with complex obstacles, the complex object shape and structure of obstacles will lead to an exponential increase in the calculation cost of unmanned vehicle path planning, which makes the unmanned vehicles Path planning efficiency is greatly reduced.

Contents of the invention

The purpose of the present invention is to provide a fast path planning method and system for an unmanned vehicle in an unknown environment with high path planning efficiency.

To achieve the above object, the present invention provides the following scheme:

The invention provides a fast path planning method for an unmanned vehicle in an unknown environment, including:

Step 1: Project the 3D point cloud data of the target environment to a ground-based binarization plane, and determine the obstacle polygon in the target environment according to the edge points on the binarization plane after projection ;

Step 2: Using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method to filter the vertices of the obstacle polygon;

The first polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the first set length; The vertices of the unmanned vehicle are filtered; the first set length is set with the minimum diameter of the unmanned vehicle itself that does not collide;

The second polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the second set length; The vertices of the obstacle are filtered; the second set length is set with the set proportional length of the longest diagonal in the minimum circumscribed quadrilateral of the obstacle polygon;

The third polygon filtering method includes, judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than the set angle; vertices to filter;

Step 3: Determine the optimal passing path of the target environment according to the obstacle polygon formed by the filtered vertices.

Optionally, before step 1, also include:

The laser radar is used to collect the three-dimensional point cloud data of the target environment.

Optionally, the step 1 specifically includes:

Based on the PCL method, the down-sampling process is carried out to the three-dimensional point cloud data;

Projecting the down-sampled 3D point cloud data onto a ground-based binarized plane to determine a binarized image of the target environment;

filtering the binarized image based on a balanced filter;

Determine the obstacle polygon in the target environment according to the edge points in the filtered binarized image.

Optionally, after step 1 and before step 2, further include:

According to the number of vertices of the obstacle polygon, it is judged whether the obstacle polygon performs polygon filtering, specifically including:

When the number of vertices of the obstacle polygon is greater than 10, performing polygon filtering on the obstacle polygon;

When the number of vertices of the obstacle polygon is less than 10, no polygon filtering is performed on the obstacle polygon.

Optionally, the step 3 specifically includes:

Determine the visible view of the target environment according to the filtered obstacle polygon;

According to the visual map, the optimal passing path of the target environment is determined based on the improved A* algorithm.

Optionally, the improved A* algorithm is specifically as follows:

F(x)=G(x)+H(x)+T(x);

Among them, T(x) is the heuristic item of passing time, G(x) is the heuristic item of the distance length from the starting node to the current node, and H(x) is the estimated cost heuristic item from the current node to the target node.

Optionally, the passing time heuristic item is specifically as follows:

；

;

为代价函数中的权重系数，/>

为无人车在节点处的航向角变化值。in,

is the weight coefficient in the cost function, />

is the heading angle change value of the unmanned vehicle at the node.

The present invention also provides a fast path planning system for an unmanned vehicle in an unknown environment, including:

The data processing module is used to project the three-dimensional point cloud data of the target environment to a binarization plane based on the ground, and determine the obstacle polygon in the target environment according to the edge point data on the binarization plane;

A filtering module, configured to filter the obstacle polygon by using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method; the first polygon filtering method includes: Judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the first set length; if so, filtering the vertices between the two adjacent sides; the first setting The length is set with the minimum diameter of the unmanned vehicle itself that does not collide; the second polygon filtering method includes: judging whether the lengths of any adjacent two sides in the obstacle polygon are less than the second setting length; if so, filter the vertices between the two adjacent sides; the second set length is set with the set proportional length of the longest diagonal in the smallest circumscribed quadrilateral of the obstacle polygon ;

A path planning module, configured to determine the optimal passing path of the target environment according to the filtered obstacle polygon.

Optionally, the data processing module includes:

A down-sampling unit, configured to perform down-sampling processing on the three-dimensional point cloud data based on the PCL method;

An image unit, configured to project the downsampled 3D point cloud data onto a ground-based binarized plane to determine a binarized image of the target environment;

a filtering unit, configured to filter the binarized image based on a balanced filter;

A drawing unit, configured to determine obstacle polygons in the target environment according to edge points in the filtered binarized image.

Optionally, the path planning module includes:

a visual view unit, configured to determine a visible view of the target environment according to the filtered obstacle polygon;

A path planning unit, configured to determine the optimal passing path of the target environment based on the improved A* algorithm according to the visual map.

According to the specific embodiments provided by the invention, the invention discloses the following technical effects:

The present invention provides a fast path planning method and system for an unmanned vehicle in an unknown environment. The method includes: projecting the three-dimensional point cloud data of the target environment onto a binarization plane based on the ground, and according to the binarization plane The edge point data on the target environment to determine the obstacle polygon; using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method to filter the obstacle polygon; the first polygon filtering method The polygon filtering method includes: judging whether the lengths of any adjacent two sides in the obstacle polygon are all less than the first set length; if so, filtering the vertices between the adjacent two sides; the first setting The length is set with the minimum diameter of the unmanned vehicle itself that does not collide; the second polygon filtering method includes: judging whether the lengths of any adjacent two sides in the obstacle polygon are less than the second set length; if so, The vertices between two adjacent sides are then filtered; the second set length is set with the set proportional length of the longest diagonal in the smallest circumscribed quadrilateral of the obstacle polygon; according to the filtered obstacle polygon , to determine the optimal path through the target environment. The present invention uses the first polygon filtering method, the second polygon filtering method and the third polygon filtering method to simplify the obstacle polygon, which reduces the amount of calculation for path planning in an environment containing complex obstacles, and improves the efficiency of path planning.

Description of drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required in the embodiments. Obviously, the accompanying drawings in the following description are only some of the present invention. Embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without paying creative labor.

Fig. 1 is the flow chart of the fast path planning method for an unmanned vehicle in an unknown environment provided by an embodiment of the present invention;

Fig. 2 is a schematic diagram of the first polygon filtering method provided by the embodiment of the present invention; Fig. 2(a) is the obstacle polygon before filtering, and Fig. 2(b) is the obstacle polygon after filtering;

Fig. 3 is a schematic diagram of the second polygon filtering method provided by the embodiment of the present invention; Fig. 3(a) is the obstacle polygon before filtering, and Fig. 3(b) is the obstacle polygon after filtering;

Fig. 4 is a schematic diagram of the third polygon filtering method provided by the embodiment of the present invention; Fig. 4(a) is the obstacle polygon before filtering, and Fig. 4(b) is the obstacle polygon after filtering;

FIG. 5 is a schematic diagram of a path planning principle provided by an embodiment of the present invention.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

The purpose of the present invention is to provide a fast path planning method and system for an unmanned vehicle in an unknown environment, which can reduce the amount of calculation for map building and path planning, and increase the speed of path planning.

In order to make the above objects, features and advantages of the present invention more comprehensible, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

As shown in Figure 1, the present invention provides a fast path planning method for an unmanned vehicle in an unknown environment, including:

Step 1: Project the 3D point cloud data of the target environment to a ground-based binarization plane, and determine the obstacle polygon in the target environment according to the edge points on the binarization plane after projection .

Step 2: Filter the vertices of the obstacle polygon by using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method.

The first polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the first set length; The vertices of the unmanned vehicle are filtered; the first set length is set with the minimum diameter of the unmanned vehicle itself that does not collide.

The second polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the second set length; The vertices of the obstacle polygon are filtered; the second set length is set with the set proportional length of the longest diagonal in the smallest circumscribed quadrilateral of the obstacle polygon.

The third polygon filtering method includes, judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than the set angle; Vertices are filtered.

Step 3: Determine the optimal passing path of the target environment according to the obstacle polygon formed by the filtered vertices.

In some embodiments, in step 1, may also include:

Based on the laser radar, the three-dimensional point cloud data of the target environment is collected.

Specifically, the lidar collects point cloud data S, establishes a point cloud map through the slam method, and then uses the PCL method to down-sample the point cloud to obtain the down-sampled point cloud map S'.

In some embodiments, the step 1 may specifically include:

Based on the PCL method, down-sampling processing is performed on the three-dimensional point cloud data.

Projecting the down-sampled 3D point cloud data onto a ground-based binarized plane to determine a binarized image of the target environment.

The binarized image is filtered based on a balanced filter.

Determine the obstacle polygon in the target environment according to the edge points in the filtered binarized image.

Wherein, based on the PCL method, the downsampling process is carried out to the three-dimensional point cloud data, specifically as follows:

The present invention can perform down-sampling processing on the three-dimensional point cloud data based on the Voxel Grid down-sampling method in the PCL method. First define the Voxel Grid filter object. Define filtering Cloud (point cloud data that needs to be down-sampled). Input the original PointCloud data (3D point cloud data) into the Voxel Grid filter. Downsampling of PointCloud data. Output filter processed point cloud.

Specifically, using the PCL method to downsample the 3D point cloud data can bring the following advantages:

1) Reduce the amount of data: Point cloud data is usually very large, and downsampling it can reduce the amount of data, making it easier to store, transmit, and process.

2) Noise removal: When collecting, reconstructing or processing point cloud data, there are often noise or abnormal points, which will 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) Improve processing efficiency: Downsampling can reduce the density of point clouds, 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 means of data preprocessing.

4) Significant release of storage space: due to the reduction of data volume, the storage burden of unnecessary data is effectively reduced.

That is, using the PCL method to down-sample the point cloud can greatly improve the processing efficiency of the point cloud data, reduce the consumption of storage and computing resources, and effectively remove noise points and improve the accuracy of the data. These factors can make point cloud data better applied to drones, autonomous driving, robots and other directions.

Wherein, based on a balanced filter, the binarized image is filtered, specifically as follows:

Balance filters reduce the effect of noise in an image by averaging the color values around each pixel in the binarized image. This filter usually uses a convolution kernel of size n×n, where n is an odd number to ensure that the center of the kernel can be located at the current pixel. The convolution operation is implemented by weighting the n×n surrounding pixels of each pixel. The average filter will also blur the details in the image while removing noise. Finally, according to the color gray value of each pixel, the gray image I ₂ can be obtained.

Wherein, the obstacle polygon in the target environment is determined according to the edge points in the filtered binarized image, specifically as follows:

个障碍物多边形的顶点/>

。Extract the edge points in the grayscale image I ₂ , and analyze the topological structure of the edge points to get the first

vertices of obstacle polygons />

.

In some embodiments, before step 2, it may also include:

According to the number of vertices of the obstacle polygon, it is judged whether polygon filtering is performed on 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. When the number of vertices of the obstacle polygon is less than 10, no polygon filtering is performed on the obstacle polygon. In the specific implementation process, the number of vertices can be set according to the actual situation. For example, the number of vertices of the obstacle polygons in the target environment is less than 10, then the threshold can be adjusted appropriately.

In this embodiment, the step 2 may specifically include:

The obstacle polygons are filtered by using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method.

是外凸角还是内凹角。first calculate a point

Whether it is convex or concave.

，/>

，/>

个顶点，首先计算第/>

个顶点的位置是否为内凹角，根据向量叉积定义，有：At this time, if the vertices of the polygon are sorted and stored counterclockwise, then for the first

, />

, />

vertices, first calculate the />th

Whether the position of a vertex is a concave angle, defined according to the vector cross product, has:

。

.

点的x坐标和y坐标。Among them, × means vector cross product; * means quantity product; x _i , y _i represent

The x-coordinate and y-coordinate of the point.

在/>

顺时针方向，即点/>

对应的角为外凸角；若上式结果小于0，则/>

在/>

逆时针方向，即点/>

对应的角为内凹角。If the result of the above formula is greater than 0, then

at />

Clockwise, i.e. dot />

The corresponding angle is the convex angle; if the result of the above formula is less than 0, then />

at />

Counterclockwise, i.e. dot />

The corresponding corners are concave corners.

处是内凹角，则过滤后无碰撞风险，继续进行下面步骤。若点/>

处是外凸角，则过滤后有碰撞风险，跳过这点/>

。if point

If there is an inner concave corner, there is no risk of collision after filtering, and proceed to the following steps. If point />

If there is an outer convex corner, there is a risk of collision after filtering, skip this point />

.

Specifically, the first polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the first set length; The vertices in between are filtered.

Specifically, the second polygon filtering method includes: judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the second set length; The vertices in between are filtered.

Specifically, the third polygon filtering method includes judging whether the concave angle formed by any two adjacent sides in the obstacle polygon is smaller than the set angle; vertices are filtered.

Among them, the first polygon filtering method is to filter with an unmanned vehicle model, which can be specifically as follows:

The vehicle occupies a circle in the map, that is, the circumscribed circle of the vehicle, with the geometric center of the vehicle as the center and the distance without collision as the radius, and the diameter of the circle is d _collision (ie the first set length).

个顶点和第/>

个顶点之间的距离/>

，公式如下：Calculate the Euclidean distance between each vertex of the obstacle polygon, for the first

vertices and />

distance between vertices />

, the formula is as follows:

。

.

点的x坐标和y坐标。Among them, x _i , y _i represent

The x-coordinate and y-coordinate of the point.

个顶点来说，和第i-1个顶点的距离为d_i-1，和第

个顶点的距离为/>

。当满足条件d_i-1<dcollision且d_i<dcollision时，将第/>

个顶点和第i个顶点相关联的边将被过滤，第i个点之后的点的标号自动减1。As shown in Figure 2(a) and Figure 2(b), for the first

For a vertex, the distance from the i-1th vertex is d i-1 , and the distance from the i-1th vertex is d _i-1 , and

The distance between vertices is />

. When the conditions d _i-1 <dcollision and d _i <dcollision are met, the />

The edge associated with the i-th vertex and the i-th vertex will be filtered, and the labels of the points after the i-th point will be automatically decremented by 1.

。After completing the filtering of the first polygon filtering method, the vertices of the obstacle polygon are obtained

.

Among them, the second polygon filtering method is to continue filtering according to the size of the obstacle, which can be as follows:

0.05 times the diagonal length of the smallest circumscribed rectangle that can include a polygon is used as the filtering threshold.

By traversing to get the maximum value x _max and minimum value x _min of the abscissa in the vertex, get the horizontal span of the polygon:

x _contour = x _max - x _min .

The same method gets the vertical span of the polygon:

y _contour = y _max - y _min .

为多边形的“规模”，计算公式如下：then set

For the "scale" of the polygon, the calculation formula is as follows:

。

.

个顶点来说，若和第/>

个顶点的距离/>

，以及和第/>

个顶点的距离/>

，当满足条件：/>

时，将第/>

个顶点和第i个顶点相关联的边将被过滤，第i个顶点之后的顶点的标号自动减1。As shown in Figure 3(a) and Figure 3(b), for the first

For a vertex, if and />

distance of vertices />

, and and the />

distance of vertices />

, when the condition is met: />

, put the />

The edge associated with the i-th vertex and the i-th vertex will be filtered, and the labels of the vertices after the i-th vertex will be automatically decremented by 1.

Specifically, the ratio can be set to 0.05, and can also be adjusted according to the actual environment and experimental conditions.

Among them, the third polygon filtering method filters the obstacle polygon according to the size of the inner angle, specifically as follows:

As shown in Figure 4(a) and Figure 4(b), in the polygon, there are some smoother partial contours that will suddenly have a concave angle inward. At this time, the vertices with too small concave angles can be deleted.

个顶点处内凹角angle的余弦值。根据向量点乘定义：/>

。Then for the i, i-1, i+1 vertices, calculate the

The cosine of the concave angle angle at vertices. According to the definition of vector dot product: />

.

表示向量点乘；*表示数量乘积；/>

分别表示/>

点的x坐标和y坐标。in,

Indicates vector dot product; * indicates quantity product; />

Respectively represent />

The x-coordinate and y-coordinate of the point.

个顶点相关联的边，第/>

个点之后的点的标号自动减1。在完成此项过滤之后，得到多边形的顶点

。Then calculate the angle according to the triangle formula at this time. Set the threshold angle _max as the maximum value of the concave angle, and take this value as 30 degrees. If the condition cos(angle)>cos(angle _max ) is satisfied, the concave angle is too small. Delete and the first

The edge associated with the vertex, the />

The labels of points after points are automatically decremented by 1. After completing this filtering, the vertices of the polygon are obtained

.

Specifically, the maximum value of the concave angle is determined according to the actual application environment, and the maximum value of the angle is the angle at which the unmanned vehicle cannot enter the concave angle.

As mentioned above, the purpose of filtering the vertices in the 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 itself, and the side that is too short in the obstacle polygon is removed. The second setting length is to filter according to the size of the obstacle polygon. When the obstacle is large, the accuracy requirement for the obstacle polygon is appropriately reduced, so as to simplify the polygon. Among them, ratio takes 0.05 as the experimental result, and can also be modified according to the specific experimental environment in practical applications.

In some embodiments, the step 3 may specifically include:

A visible view of the target environment is determined according to the filtered obstacle polygon.

According to the visual map, the optimal passing path of the target environment is determined based on the improved A* algorithm.

Wherein, according to the filtered obstacle polygon, the visible view of the target environment is determined, specifically as follows:

构建可视图。Contour feature points passing through filtered obstacles

Build visualizations.

In the local map, the obstacle contour points are used as vertices in the visible view, and the adjacent vertices of the same obstacle form the edges of obstacles in the visible view.

For polygonal maps, there are local maps and global maps. For vertices in the local map and vertices in the global map, if the Euclidean distance between the two vertices is less than a certain threshold, it is assumed that the two vertices actually represent the same coordinates point and link them. After the local map is obtained through the previous steps, the global map is updated every once in a while.

Wherein, according to the visual diagram, based on the improved A* algorithm, the optimal passing path of the target environment is determined, specifically as follows:

。根据节点处路径航向角的变化大小生成基于安全性的启发项/>

。此时每个节点处的启发函数为：/>

；其中/>

为传统A*算法启发函数中的启发项。Use the improved A* algorithm to plan navigation based on the visual map. Considering the speed and safety of navigation, heuristics based on passing time and safety are added to the algorithm. At the node, according to the time required for the vehicle to pass through the arc here, a heuristic item based on the passing time will be generated based on the time

. Generate safety-based heuristics based on the magnitude of change in path heading angle at nodes />

. At this time, the heuristic function at each node is: />

; where />

is the heuristic item in the traditional A* algorithm heuristic function.

点时，对于后续点的启发项目将添加/>

，

，式中，/>

为代价函数中的权重系数。/>

为在此顶点前后路径的夹角，即车辆在顶点处的航向角变化值。例如角度/>

为/>

，/>

的夹角，车辆的航向角变化最小，车辆行驶路径的平滑度最高，通过顶点时所需要转向的时间也最小，/>

的值就最小。图中蓝色的三条待选择路径，只对于/>

来说，/>

路径所得到启发函数的值越小，/>

路径所得到启发函数的值越大。Take Figure 5 as an example, when the planning program runs to

point, heuristic items for subsequent points will add />

,

, where, />

is the weight coefficient in the cost function. />

is the included angle of the path before and after this vertex, that is, the heading angle change value of the vehicle at the vertex. e.g. angle />

for />

, />

The included angle, the heading angle of the vehicle changes the least, the smoothness of the vehicle's driving path is the highest, and the time required to turn when passing the apex is also the smallest, />

value is the smallest. The three blue paths to be selected in the figure are only for />

say, />

The smaller the value of the heuristic function obtained by the path, the />

The value of the heuristic function obtained by the path is larger.

In summary, when the selected path has a smaller change in heading angle at the vertex, the smoother the vehicle path, the less time it takes for the vehicle to pass through, and the higher the safety, the more conducive the vehicle can pass quickly and safely. At this time, the value of the heuristic function is smaller. Plan according to the heuristic function, so as to obtain an optimal path that guarantees the navigation speed and safety at the same time.

The present invention also provides a fast path planning system for an unmanned vehicle in an unknown environment, including:

The data processing module is used to project the 3D point cloud data of the target environment to a ground-based binarization plane, and determine the obstacle polygon in the target environment according to the edge point data on the binarization plane.

A filtering module, configured to filter the obstacle polygon by using the first polygon filtering method, the second polygon filtering method and the third polygon filtering method; the first polygon filtering method includes: Judging whether the lengths of any two adjacent sides in the obstacle polygon are less than the first set length; if so, filtering the vertices between the two adjacent sides; the first setting The length is set with the minimum diameter of the unmanned vehicle itself that does not collide; the second polygon filtering method includes: judging whether the lengths of any adjacent two sides in the obstacle polygon are less than the second setting length; if so, filter the vertices between the two adjacent sides; the second set length is set with the set proportional length of the longest diagonal in the smallest circumscribed quadrilateral of the obstacle polygon .

A path planning module, configured to determine the optimal passing path of the target environment according to the filtered obstacle polygon.

In some embodiments, the data processing module may include:

The down-sampling unit is configured to perform down-sampling processing on the three-dimensional point cloud data based on the PCL method.

The image unit is configured to project the down-sampled 3D point cloud data onto a ground-based binarized plane to determine a binarized image of the target environment.

A filtering unit is configured to filter the binarized image based on a balanced filter.

A drawing unit, configured to determine obstacle polygons in the target environment according to edge points in the filtered binarized image.

In some embodiments, the path planning module may include:

The visible view unit is configured to determine the visible view of the target environment according to the filtered obstacle polygon.

A path planning unit, configured to determine the optimal passing path of the target environment based on the improved A* algorithm according to the visual map.

In summary, the present invention has the following advantages:

(1) The present invention filters polygons based on the vehicle model and the size of obstacles, and adds a filtering method for concave corner vertices, which simplifies the polygons in the map, reduces the amount of calculations for subsequent mapping and planning, and improves speed of path planning.

(2) The present invention is based on the visual graph-based mapping and the traditional A* algorithm, and proposes an improved A* algorithm based on the visual graph. On the basis of the shortest distance, heuristic items based on passing time and security can be added to obtain the optimal The route can reduce the navigation time and ensure the safety of the vehicle while driving.

Each embodiment in this specification is described in a progressive manner, each embodiment focuses on the difference from other embodiments, and the same and similar parts of each embodiment can be referred to each other. As for the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and for the related information, please refer to the description of the method part.

In this paper, specific examples have been used to illustrate the principle and implementation of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; meanwhile, for those of ordinary skill in the art, according to the present invention Thoughts, there will be changes in specific implementation methods and application ranges. In summary, the contents of this specification should not be construed as limiting the present 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:

；

wherein ,

for the weight coefficient in the cost function, +.>

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.

Images