A path planning method and terminal for unmanned vehicle

Abstract

The present invention discloses a path planning method and terminal for an unmanned vehicle. When there is a passable path in a known driving area, a designated path point set of the driving area is received, and all passable path points are connected in pairs to obtain passable path connection line segments; the passable path connection line segments and the connection line segments of each pair of adjacent path points are intersected and judged. If they do not intersect, it means that there are no roads and obstacles between the adjacent path points, and point filling processing can be performed. If they intersect, it means that there are roads between the adjacent path points. A first path is planned according to the passable path point set, and in order to avoid path duplication, the passable path points in the first path are deleted from the passable path point set. Therefore, when there is a passable path in a known driving area, the shortest planned route can be obtained by combining the designated path point set.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to an unmanned vehicle path planning method and terminal. Background Art

[0002] In the prior art, when operators plan the route for unmanned vehicles on the command platform, they will encounter complex obstacle areas. It is known that there are passable paths in the obstacle area, and the paths are irregular. The paths may overlap multiple times and there are no clear entrances and exits. At this time, the unmanned vehicle path planned by the unmanned vehicle usually has repeated areas, and it is difficult to find the shortest path. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a path planning method and terminal for an unmanned vehicle, which can plan the shortest path from existing paths according to given path points.

[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0005] A method for unmanned vehicle path planning, comprising the steps of:

[0006] Receive a set of designated waypoints in a driving area;

[0007] Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments;

[0008] Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set;

[0009] Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set.

[0010] In order to solve the above technical problems, another technical solution adopted by the present invention is:

[0011] A path planning terminal for an unmanned vehicle includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program:

[0012] Receive a set of designated waypoints in a driving area;

[0013] Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments;

[0014] Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set;

[0015] Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set.

[0016] The beneficial effects of the present invention are as follows: when there is a passable path in a known driving area, a designated path point set of the driving area is received, and all passable path points are connected in pairs to obtain passable path connecting line segments; the passable path connecting line segments and the connecting line segments of each pair of adjacent path points are intersected and judged, if they do not intersect, it means that there are no roads and obstacles between the adjacent path points, and point filling processing can be performed; if they intersect, it means that there are roads between the adjacent path points, and a first path is planned according to the passable path point set, and in order to avoid path duplication, the passable path points in the first path are deleted from the passable path point set. Therefore, when there is a passable path in a known driving area, the shortest planned route can be obtained by combining the designated path point set. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 A flow chart of a method for path planning of an unmanned vehicle according to an embodiment of the present invention;

[0018] Figure 2 A schematic diagram of an unmanned vehicle path planning terminal according to an embodiment of the present invention;

[0019] Figure 3 A flowchart of specific steps of a method for path planning for an unmanned vehicle according to an embodiment of the present invention;

[0020] Figure 4 A schematic diagram of a path planning method for an unmanned vehicle according to an embodiment of the present invention;

[0021] Description of labels:

[0022] 1. An unmanned vehicle path planning terminal; 2. Memory; 3. Processor. DETAILED DESCRIPTION

[0023] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0024] Please refer to Figure 1 , an embodiment of the present invention provides a method for unmanned vehicle path planning, comprising the steps of:

[0025] Receive a set of designated waypoints in a driving area;

[0026] Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments;

[0027] Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set;

[0028] Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set.

[0029] From the above description, it can be seen that the beneficial effects of the present invention are: when there is a passable path in a known driving area, a designated path point set of the driving area is received, and all passable path points are connected in pairs to obtain passable path connecting line segments; the passable path connecting line segments and the connecting line segments of each pair of adjacent path points are intersected and judged. If they do not intersect, it means that there are no roads and obstacles between the adjacent path points, and point filling can be performed. If they intersect, it means that there are roads between the adjacent path points. The first path is planned according to the passable path point set, and in order to avoid path duplication, the passable path points in the first path are deleted from the passable path point set. Therefore, when there is a passable path in a known driving area, the shortest planned route can be obtained in combination with the designated path point set.

[0030] Further, selecting respectively the first traversable path starting point and the first traversable path end point which are closest to two currently adjacent path points in the traversable path point set comprises:

[0031] Calculate the distances between all traversable path points, and if the distances between the traversable path points are less than or equal to the preset distance, store the distances between the traversable path points in the first matrix;

[0032] Based on the first matrix, a first traversable path starting point and a first traversable path end point which are closest to two currently adjacent path points are respectively selected.

[0033] It can be seen from the above description that when the distance between the passable path points is less than or equal to the preset distance, storing it in the first matrix can effectively save the distance from each point to other points in the channel path point set.

[0034] Further, determining the first path according to the passable path point set includes:

[0035] Traversing the passable path point set, calculating the distances between all passable path points, and storing the distances between the passable path points in a second matrix;

[0036] A first path is calculated based on the second matrix, a first traversable path starting point and a first traversable path end point.

[0037] From the above description, it can be seen that by performing a secondary traversal loop on the set of traversable path points, the distances between all traversable path points are calculated to obtain a second matrix. Therefore, by combining the second matrix, the starting point of the first traversable path and the end point of the first traversable path, the shortest first path can be calculated.

[0038] Further, after determining the first path according to the passable path point set, the method further comprises:

[0039] The adjacent path points include a first starting point and a first end point;

[0040] A point is added between the first starting point and the starting point of the first traversable path, and a point is added between the first end point and the end point of the first traversable path.

[0041] It can be seen from the above description that filling in points between the first starting point and the starting point of the first traversable path, and filling in points between the first end point and the end point of the first traversable path, can ensure the reliability of the planned path.

[0042] Further, supplementing the adjacent path points, and adding the path points obtained by supplementing the adjacent path points and the adjacent path points to the planned path point set includes:

[0043] Determine whether the distance between the adjacent path points exceeds a threshold value. If so, fill in the adjacent path points and add the path points obtained by filling in the points and the adjacent path points to the planned path point set. Otherwise, directly add the adjacent path points to the planned path point set.

[0044] Delete the same path points in the planned path point set.

[0045] From the above description, it can be seen that when the connecting line segments of the traversable path and the connecting line segments of adjacent path points do not intersect, if the distance between adjacent path points exceeds the threshold, points are added to ensure the reliability of the planned path.

[0046] Please refer to Figure 2 Another embodiment of the present invention provides an unmanned vehicle path planning terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the following steps when executing the computer program:

[0047] Receive a set of designated waypoints in a driving area;

[0048] Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments;

[0049] Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set;

[0050] Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set.

[0051] From the above description, it can be seen that the beneficial effects of the present invention are: when there is a passable path in a known driving area, a designated path point set of the driving area is received, and all passable path points are connected in pairs to obtain passable path connecting line segments; the passable path connecting line segments and the connecting line segments of each pair of adjacent path points are intersected and judged. If they do not intersect, it means that there are no roads and obstacles between the adjacent path points, and point filling can be performed. If they intersect, it means that there are roads between the adjacent path points. The first path is planned according to the passable path point set, and in order to avoid path duplication, the passable path points in the first path are deleted from the passable path point set. Therefore, when there is a passable path in a known driving area, the shortest planned route can be obtained in combination with the designated path point set.

[0052] Further, selecting respectively the first traversable path starting point and the first traversable path end point which are closest to two currently adjacent path points in the traversable path point set comprises:

[0053] Calculate the distances between all traversable path points, and if the distances between the traversable path points are less than or equal to the preset distance, store the distances between the traversable path points in the first matrix;

[0054] Based on the first matrix, a first traversable path starting point and a first traversable path end point which are closest to two currently adjacent path points are respectively selected.

[0055] It can be seen from the above description that when the distance between the passable path points is less than or equal to the preset distance, storing it in the first matrix can effectively save the distance from each point to other points in the channel path point set.

[0056] Further, determining the first path according to the passable path point set includes:

[0057] Traversing the passable path point set, calculating the distances between all passable path points, and storing the distances between the passable path points in a second matrix;

[0058] A first path is calculated based on the second matrix, a first traversable path starting point and a first traversable path end point.

[0059] From the above description, it can be seen that by performing a secondary traversal loop on the set of traversable path points, the distances between all traversable path points are calculated to obtain a second matrix. Therefore, by combining the second matrix, the starting point of the first traversable path and the end point of the first traversable path, the shortest first path can be calculated.

[0060] Further, after determining the first path according to the passable path point set, the method further comprises:

[0061] The adjacent path points include a first starting point and a first end point;

[0062] A point is added between the first starting point and the starting point of the first traversable path, and a point is added between the first end point and the end point of the first traversable path.

[0063] It can be seen from the above description that filling in points between the first starting point and the starting point of the first traversable path, and filling in points between the first end point and the end point of the first traversable path, can ensure the reliability of the planned path.

[0064] Further, supplementing the adjacent path points, and adding the path points obtained by supplementing the adjacent path points and the adjacent path points to the planned path point set includes:

[0065] Determine whether the distance between the adjacent path points exceeds a threshold value. If so, fill in the adjacent path points and add the path points obtained by filling in the points and the adjacent path points to the planned path point set. Otherwise, directly add the adjacent path points to the planned path point set.

[0066] Delete the same path points in the planned path point set.

[0067] From the above description, it can be seen that when the connecting line segments of the traversable path and the connecting line segments of adjacent path points do not intersect, if the distance between adjacent path points exceeds the threshold, points are added to ensure the reliability of the planned path.

[0068] The above-mentioned unmanned vehicle path planning method and terminal of the present invention are applicable to a known obstacle area with a passable channel path, and the channel path is irregular, the channel path may overlap multiple times, and there is no clear entrance and exit. According to the given path points, the shortest path is planned from the existing paths. The following is an explanation through a specific implementation method:

[0069] Embodiment 1

[0070] Please refer to Figure 1 and Figure 3 , a method for unmanned vehicle path planning, comprising the steps of:

[0071] S1. Receive a set of designated path points in a driving area.

[0072] Specifically, in this embodiment, please refer to Figure 4 , the operator marks the designated path points on the map at obstacle-free locations by marking on the touch platform, i.e. Figure 4 The triangular path points in the grid are used to obtain a set of longitude and latitude: pArr = [p0, p1, p2...pN], where p0 represents the starting point, pN represents the end point, and the remaining points represent the waypoints that must be passed.

[0073] S2. Obtain a set of passable path points in the driving area, connect all passable path points in pairs, and obtain passable path connection line segments.

[0074] Specifically, it is assumed that there is a passable path channel in the driving area on the touch platform, that is, Figure 4 The circular path points in the channel are connected in pairs to form a line segment lineR.

[0075] Preferably, the driving area includes an overall boundary, and all traversable path points in the traversable path point set are located within the overall boundary, that is, there are traversable path points around each obstacle in the driving area.

[0076] S3, connecting adjacent path points in the specified path point set, and determining whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, if so, executing step S4, otherwise, executing step S5.

[0077] Specifically, in this embodiment, pArr is traversed from p0 to pN-1, then: point pIn=p(i), point pOut=p(i+1), the line segment between points pIn and pOut is lineIO, and it is determined whether there is an intersection between lineR and lineIO. If there is an intersection, step S4 is executed, otherwise, step S5 is executed.

[0078] S4. Select the first traversable path starting point and the first traversable path end point which are closest to two currently adjacent path points from the traversable path point set respectively, determine the first path according to the traversable path point set, add the first path to the planned path point set, and delete the traversable path points in the first path from the traversable path point set.

[0079] In this embodiment, if there is an intersection, it is considered that point pIn can reach point pOut along the lineR channel, and the shortest path point in the lineR channel is found according to the following steps:

[0080] S41. Calculate the distances between all traversable path points. If the distances between the traversable path points are less than or equal to a preset distance, store the distances between the traversable path points in a first matrix.

[0081] Specifically, the points in the channel path point set rArr are traversed in pairs to generate the first matrix martix1. In the process of generating the matrix, the distance between each two points is judged: if the distance between the two points is greater than the preset distance, it is set to an invalid value, that is, it is considered that the unmanned vehicle cannot reach point B from point A due to the distance being too far during its driving, so A->B is invalid; otherwise, if it is less than or equal to the preset distance, it is considered valid and the distance value is recorded in the matrix. The significance of this matrix is ​​to save the distance from each point in the channel path point set to other points.

[0082] S42: Based on the first matrix, respectively select a first traversable path starting point and a first traversable path end point that are closest to two currently adjacent path points.

[0083] Specifically, the open source spatial geographic analysis library Turf.js is introduced, and the nearsetPoint method is used to find the point startNeast closest to pIn and the point endNeast closest to pOut in the lineR segment.

[0084] S43, traversing the set of passable path points, calculating the distances between all passable path points, and storing the distances between the passable path points in a second matrix.

[0085] Specifically, the channel path point set rArr is traversed twice, and the distances between points in rArr and other points in rArr are calculated pairwise to obtain a second matrix martix2 based on the expansion of rArr.

[0086] S44. Calculate a first path based on the second matrix, a starting point of a first traversable path, and an end point of the first traversable path.

[0087] Specifically, the open source Dijkstra algorithm library Dijkstra.js is introduced. Dijkstra algorithm is a greedy shortest path algorithm that uses breadth-first search to solve the single-source shortest path problem of weighted directed or undirected graphs, and finally obtains a shortest path tree.

[0088] By calling the Dijkstra algorithm library, the matrix martix2, the starting point startNeast and the end point endNeast, we can get a set of shortest path points shortestP=[s0,s1,s2,...,sN] starting from startNeast and ending at endNeast, that is, Figure 4 The square waypoints in .

[0089] S45. Fill in a point between the first starting point and the starting point of the first traversable path, and fill in a point between the first end point and the end point of the first traversable path.

[0090] Specifically, in this embodiment, the first starting point is point pIn, the first end point is point pOut, point pIn is added to the startNeast point to generate a path point set pIntArr, point pOut is added to the endNeast point to generate a path point set pOutArr, and then the three path point sets pIntArr, shortestP, and pOutArr are added to path. Finally, the shortest planned path starting from point pIn and reaching point pOut along the lineR channel is generated.

[0091] S46: Delete the traversable path points in the first path from the traversable path point set.

[0092] Specifically, in the actual operation process, it is required that after the unmanned vehicle drives to a certain route point in the channel, it cannot return along the way it came and track to another route point. Therefore, after the shortest route between two route points is completed, the points in the obtained shortest path point set shortestP must be removed from the channel path point set rArr before the shortest route planning between the two route points in the next stage can be carried out.

[0093] S5. Fill in the adjacent path points, and add the path points obtained by filling in the adjacent path points to the planned path point set.

[0094] Specifically, if there is no intersection, it is considered that there is no passing road between point pIn and point pOut, and it is considered that there is no obstacle between point pIn and point pOut. If the distance between point pIn and point pOut is greater than the threshold, the two points are directly filled in to form a planned route from pIn to pOut, and the generated path point set is added to the path set.

[0095] Also, delete the path points that are identical to the planned path point set.

[0096] Embodiment 2

[0097] Please refer to Figure 2 , an unmanned vehicle path planning terminal 1, including a memory 2, a processor 3, and a computer program stored in the memory 2 and executable on the processor 3. When the processor 3 executes the computer program, each step of an unmanned vehicle path planning method of embodiment 1 is implemented.

[0098] In summary, the present invention provides a method and terminal for planning the path of an unmanned vehicle. When there is a passable path in a known driving area, a designated path point set of the driving area is received, and all passable path points are connected in pairs to obtain passable path connecting line segments; the passable path connecting line segments and the connecting line segments of each pair of adjacent path points are intersected and judged. If they do not intersect, it means that there are no roads and obstacles between the adjacent path points, and point filling can be performed. If they intersect, it means that there are roads between the adjacent path points. A first path is planned according to the passable path point set, and in order to avoid path duplication, the passable path points in the first path are deleted from the passable path point set. Therefore, when there is a passable path in a known driving area, the shortest planned route can be obtained by combining the designated path point set.

[0099] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for unmanned vehicle path planning, characterized in that: Includes steps: Receive a set of designated waypoints in a driving area; Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments; Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set; Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set; Selecting respectively from the set of passable path points a first passable path starting point and a first passable path end point which are closest to two currently adjacent path points comprises: Calculate the distances between all traversable path points, and if the distances between the traversable path points are less than or equal to the preset distance, store the distances between the traversable path points in the first matrix; Based on the first matrix, a first traversable path starting point and a first traversable path end point which are closest to two currently adjacent path points are respectively selected.

2. The unmanned vehicle path planning method according to claim 1, characterized in that: Determining the first path according to the passable path point set includes: Traversing the passable path point set, calculating the distances between all passable path points, and storing the distances between the passable path points in a second matrix; A first path is calculated based on the second matrix, a first traversable path starting point and a first traversable path end point.

3. The unmanned vehicle path planning method according to claim 1, characterized in that: After determining the first path according to the passable path point set, the method further comprises: The adjacent path points include a first starting point and a first end point; A point is added between the first starting point and the starting point of the first traversable path, and a point is added between the first end point and the end point of the first traversable path.

4. The unmanned vehicle path planning method according to claim 1, characterized in that: Filling the adjacent path points, and adding the path points obtained by filling the adjacent path points and the adjacent path points to the planned path point set includes: Determine whether the distance between the adjacent path points exceeds a threshold value. If so, fill in the adjacent path points and add the path points obtained by filling in the points and the adjacent path points to the planned path point set. Otherwise, directly add the adjacent path points to the planned path point set. Delete the same path points in the planned path point set.

5. An unmanned vehicle path planning terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the following steps are implemented: Receive a set of designated waypoints in a driving area; Obtaining a set of passable path points in the driving area, connecting all passable path points in pairs to obtain passable path connection line segments; Connect adjacent path points in the specified path point set, determine whether the connecting line segment of each pair of adjacent path points intersects with the connecting line segment of the passable path, and if so, select a first passable path starting point and a first passable path end point that are closest to the two currently adjacent path points in the passable path point set, determine a first path based on the passable path point set, add the first path to the planned path point set, and delete the passable path points in the first path from the passable path point set; Otherwise, the adjacent path points are supplemented, and the path points obtained by supplementing and the adjacent path points are added to the planned path point set; Selecting respectively from the set of passable path points a first passable path starting point and a first passable path end point which are closest to two currently adjacent path points comprises: Calculate the distances between all traversable path points, and if the distances between the traversable path points are less than or equal to the preset distance, store the distances between the traversable path points in the first matrix; Based on the first matrix, a first traversable path starting point and a first traversable path end point which are closest to two currently adjacent path points are respectively selected.

6. The unmanned vehicle path planning terminal according to claim 5, characterized in that: Determining the first path according to the passable path point set includes: Traversing the passable path point set, calculating the distances between all passable path points, and storing the distances between the passable path points in a second matrix; A first path is calculated based on the second matrix, a first traversable path starting point and a first traversable path end point.

7. The unmanned vehicle path planning terminal according to claim 5, characterized in that: After determining the first path according to the passable path point set, the method further comprises: The adjacent path points include a first starting point and a first end point; A point is added between the first starting point and the starting point of the first traversable path, and a point is added between the first end point and the end point of the first traversable path.

8. The unmanned vehicle path planning terminal according to claim 5, characterized in that: Filling the adjacent path points, and adding the path points obtained by filling the adjacent path points and the adjacent path points to the planned path point set includes: Determine whether the distance between the adjacent path points exceeds a threshold value. If so, fill in the adjacent path points and add the path points obtained by filling in the points and the adjacent path points to the planned path point set. Otherwise, directly add the adjacent path points to the planned path point set. Delete the same path points in the planned path point set.

Images