Detailed Description
In order to describe the technical contents, the achieved objects and effects of the present invention in detail, the following description will be made with reference to the embodiments in conjunction with the accompanying drawings.
Referring to fig. 1, an embodiment of the present invention provides a method for planning a path of an unmanned vehicle, including the steps of:
Receiving a designated path point set of a driving area;
Acquiring a passable path point set of the driving area, and connecting all passable path points in pairs to obtain passable path connecting line segments;
connecting adjacent path points in the appointed path point set, judging whether the connecting line segment of each pair of adjacent path points is intersected with the passable path connecting line segment, if so, respectively selecting a first passable path starting point and a first passable path ending point which are nearest to two current adjacent path points in the passable path point set, determining a first path according to the passable path point set, adding the first path into a planning path point set, and deleting the passable path points in the first path from the passable path point set;
Otherwise, the adjacent path points are subjected to point complementation, and the path points obtained by the point complementation and the adjacent path points are added into a planning path point set.
From the above description, the method has the advantages that when a passable path exists in a known driving area, a designated path point set of the driving area is received, 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 the passable path connecting line segments are not intersected, no road or obstacle exists between the adjacent path points, point supplementing processing can be performed, if the passable path points are intersected, a first path is planned according to the passable path point set, and in order to avoid path repetition, the passable path points in the first path are deleted from the passable path point set. Therefore, when the trafficable path exists in the known driving area, the shortest planning route can be obtained by combining the designated path point set.
Further, selecting the first traversable path start point and the first traversable path end point closest to the two currently adjacent path points in the traversable path point set respectively includes:
Calculating the distance between all the passable path points, and if the distance between the passable path points is smaller than or equal to the preset distance, storing the distance between the passable path points into a first matrix;
And respectively selecting a first passable path starting point and a first passable path ending point which are nearest to the two current adjacent path points based on the first matrix.
As can be seen from the above description, when the distance between the passable path points is less than or equal to the preset distance, the distance between each point in the set of path points of the channel and other points can be effectively saved by storing the distance in the first matrix.
Further, determining the first path from the set of passable path points includes:
traversing the passable path point set, calculating the distance between all passable path points, and storing the distance between the passable path points into a second matrix;
And calculating to obtain a first path based on the second matrix, the first traversable path starting point and the first traversable path ending point.
As can be seen from the above description, by performing a second traversal cycle on the passable path point set, the distances between all passable path points are calculated to obtain a second matrix, so that the shortest first path can be calculated by combining the second matrix, the first passable path start point and the first passable path end point.
Further, determining the first path according to the passable path point set includes:
The adjacent path points comprise a first starting point and a first ending point;
and carrying out point complementation between the first starting point and the first traversable path starting point, and carrying out point complementation between the first ending point and the first traversable path ending point.
As can be seen from the above description, the point is complemented between the first starting point and the first traversable path starting point, and the point is complemented between the first ending point and the first traversable path ending point, so that the reliability of the planned path can be ensured.
Further, the supplementing the adjacent path points, and adding the path points obtained by the supplementing and the adjacent path points to the planning path point set includes:
Judging whether the distance between the adjacent path points exceeds a threshold value, if so, carrying out point complement on the adjacent path points, adding the path points obtained by the point complement and the adjacent path points into a planning path point set, and if not, directly adding the adjacent path points into the planning path point set;
And deleting the same path points of the planning path point set.
From the above description, when the passable path connecting line segment and the connecting line segment of the adjacent path point are not intersected, if the distance between the adjacent path points exceeds the threshold value, the points are complemented, so that the reliability of the planned path can be ensured.
Referring to fig. 2, another embodiment of the present invention provides an unmanned vehicle path planning terminal, including 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:
Receiving a designated path point set of a driving area;
Acquiring a passable path point set of the driving area, and connecting all passable path points in pairs to obtain passable path connecting line segments;
connecting adjacent path points in the appointed path point set, judging whether the connecting line segment of each pair of adjacent path points is intersected with the passable path connecting line segment, if so, respectively selecting a first passable path starting point and a first passable path ending point which are nearest to two current adjacent path points in the passable path point set, determining a first path according to the passable path point set, adding the first path into a planning path point set, and deleting the passable path points in the first path from the passable path point set;
Otherwise, the adjacent path points are subjected to point complementation, and the path points obtained by the point complementation and the adjacent path points are added into a planning path point set.
From the above description, the method has the advantages that when a passable path exists in a known driving area, a designated path point set of the driving area is received, 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 the passable path connecting line segments are not intersected, no road or obstacle exists between the adjacent path points, point supplementing processing can be performed, if the passable path points are intersected, a first path is planned according to the passable path point set, and in order to avoid path repetition, the passable path points in the first path are deleted from the passable path point set. Therefore, when the trafficable path exists in the known driving area, the shortest planning route can be obtained by combining the designated path point set.
Further, selecting the first traversable path start point and the first traversable path end point closest to the two currently adjacent path points in the traversable path point set respectively includes:
Calculating the distance between all the passable path points, and if the distance between the passable path points is smaller than or equal to the preset distance, storing the distance between the passable path points into a first matrix;
And respectively selecting a first passable path starting point and a first passable path ending point which are nearest to the two current adjacent path points based on the first matrix.
As can be seen from the above description, when the distance between the passable path points is less than or equal to the preset distance, the distance between each point in the set of path points of the channel and other points can be effectively saved by storing the distance in the first matrix.
Further, determining the first path from the set of passable path points includes:
traversing the passable path point set, calculating the distance between all passable path points, and storing the distance between the passable path points into a second matrix;
And calculating to obtain a first path based on the second matrix, the first traversable path starting point and the first traversable path ending point.
As can be seen from the above description, by performing a second traversal cycle on the passable path point set, the distances between all passable path points are calculated to obtain a second matrix, so that the shortest first path can be calculated by combining the second matrix, the first passable path start point and the first passable path end point.
Further, determining the first path according to the passable path point set includes:
The adjacent path points comprise a first starting point and a first ending point;
and carrying out point complementation between the first starting point and the first traversable path starting point, and carrying out point complementation between the first ending point and the first traversable path ending point.
As can be seen from the above description, the point is complemented between the first starting point and the first traversable path starting point, and the point is complemented between the first ending point and the first traversable path ending point, so that the reliability of the planned path can be ensured.
Further, the supplementing the adjacent path points, and adding the path points obtained by the supplementing and the adjacent path points to the planning path point set includes:
Judging whether the distance between the adjacent path points exceeds a threshold value, if so, carrying out point complement on the adjacent path points, adding the path points obtained by the point complement and the adjacent path points into a planning path point set, and if not, directly adding the adjacent path points into the planning path point set;
And deleting the same path points of the planning path point set.
From the above description, when the passable path connecting line segment and the connecting line segment of the adjacent path point are not intersected, if the distance between the adjacent path points exceeds the threshold value, the points are complemented, so that the reliability of the planned path can be ensured.
The unmanned vehicle path planning method and the terminal are suitable for the channel paths which can pass through the known obstacle area, the channel paths are irregular, the channel paths can be overlapped for many times, when no clear entrance and exit exist, the shortest path is planned from the existing paths according to the given path points, and the method and the terminal are described in the following specific embodiments:
Example 1
Referring to fig. 1 and 3, a method for planning a path of an unmanned vehicle includes the steps of:
s1, receiving a designated path point set of a driving area.
Specifically, in this embodiment, referring to fig. 4, an operator marks a designated path point on a map in a manner of marking on the touch platform, that is, a triangle path point in fig. 4, to obtain a longitude and latitude set pArr = [ p0, p1, p2...pn ], where p0 represents a start point, pN represents an end point, and the rest points represent path points that must be passed.
S2, acquiring a passable path point set of the driving area, and connecting all passable path points in pairs to obtain passable path connecting line segments.
Specifically, a passable path channel in the existing driving area on the touch platform, that is, a circular path point in fig. 4, is set, where the passable path point set is rArr = [ r0, r1, r2...rn ], and all the path points in the channel are connected in pairs to form a line segment lineR.
Preferably, the travel area includes an overall boundary within which all of the passable path points in the set of passable path points lie, i.e., passable path points are located around each obstacle in the travel area.
And S3, connecting adjacent path points in the designated path point set, judging whether the connecting line segments of each pair of adjacent path points intersect with the passable path connecting line segments, if so, executing the step S4, otherwise, executing the step S5.
Specifically, in the present embodiment, traversing pArr from p0 to pN-1, if the point pIn =p (i), the point pOut =p (i+1), the line segment between the pIn and the pOut is lineIO, it is determined whether there is an intersection between lineR and lineIO, if so, step S4 is executed, otherwise, step S5 is executed.
S4, respectively selecting a first passable path starting point and a first passable path ending point which are closest to two current adjacent path points from the passable path point set, determining a first path according to the passable path point set, adding the first path into a planning path point set, and deleting the passable path points in the first path from the passable path point set.
In this embodiment, if there is an intersection, it is considered that the point pIn can reach the point pOut along the lineR channel, and the shortest path point in the lineR channel is found as follows:
S41, calculating the distance between all the passable path points, and if the distance between the passable path points is smaller than or equal to the preset distance, storing the distance between the passable path points into a first matrix.
Specifically, the points in the channel path point set rArr are traversed two by two, generating a first matrix martix. In the process of generating the matrix, judging the distance between every two points, if the distance between the two points is larger than the preset distance, setting the distance as an invalid value, namely considering that the distance is too far to reach the point B from the point A in the running process of the unmanned vehicle, so that the point A- > B is invalid, otherwise, considering that the distance is smaller than or equal to the preset distance, and recording the distance value in the matrix. The matrix has the meaning of preserving the distances from each point to other points in the set of path points.
S42, respectively selecting a first passable path starting point and a first passable path ending point which are nearest to two current adjacent path points based on the first matrix.
Specifically, an open source spatial geographic analysis library turf. Js is introduced, using the nearsetPoint method to find the point STARTNEAST closest to pIn and the point endNeast closest to pOut in the lineR line segment.
S43, traversing the passable path point set, calculating the distance between all passable path points, and storing the distance between the passable path points into a second matrix.
Specifically, a secondary traversal cycle is performed on the set rArr of path points, and the distances between the points in rArr and other points in rArr are calculated two by two, so as to obtain a second matrix martix2 based on rArr expansion.
S44, calculating to obtain a first path based on the second matrix, the first passable path starting point and the first passable path ending point.
Specifically, the Dijkstra.js algorithm library of open source Dijkstra is introduced. The Di Jie Style algorithm is a greedy shortest path algorithm, and the problem of single-source shortest paths of weighted directed graphs or undirected graphs is solved by using breadth-first search, so that a shortest path tree can be finally obtained.
By invoking the dijkstra algorithm library, matrix martix2, and start STARTNEAST and end endNeast, a set of shortest path points shortestP = [ s0, s1, s2, ], sN ], starting from STARTNEAST, to endNeast, can be obtained, i.e. square path points in fig. 4.
S45, performing point complementation between the first starting point and the first traversable path starting point, and performing point complementation between the first ending point and the first traversable path ending point.
Specifically, in this embodiment, the first starting point is pIn points, the first end point is pOut points, the pIn points and STARTNEAST points are complemented to generate a path point set pIntArr, the pOut points and endNeast points are complemented to generate a path point set pOutArr, and the pIntArr, shortestP, pOutArr three path point sets are added to the path. The final generation begins at point pIn and follows lineR the shortest planned path to point pOut.
S46, deleting passable path points in the first path from the passable path point set.
Specifically, in the actual running process, after the unmanned vehicle is required to travel to a certain route point in the channel, the unmanned vehicle cannot return to and re-track to another route point along the incoming route. Therefore, after the shortest route between two route points is ended, the points in the obtained shortest route point set shortestP are removed from the channel route point set rArr, so that the shortest route planning of two route points in the next stage can be performed.
S5, carrying out point complement on the adjacent path points, and adding the path points obtained by the point complement and the adjacent path points into a planning path point set.
Specifically, if there is no intersection, it is considered that there is no route between the point pIn and the point pOut, and no obstacle is considered between the point pIn and the point pOut, if the distance between the point pIn and the point pOut is greater than the threshold value, the two points are directly complemented to form a planned route from pIn to pOut, and the generated route point set is added to the path set.
And deleting the same route points of the planning route point set.
Example two
Referring to fig. 2, an unmanned vehicle path planning terminal 1 includes a memory 2, a processor 3, and a computer program stored in the memory 2 and executable on the processor 3, wherein the processor 3 implements the steps of an unmanned vehicle path planning method according to the first embodiment when executing the computer program.
In summary, the unmanned vehicle path planning method and terminal provided by the invention are used for receiving a designated path point set of a driving area when a passable path exists in the known driving area, connecting all passable path points in pairs to obtain passable path connecting line segments, judging intersection of the passable path connecting line segments and the connecting line segments of each pair of adjacent path points, if the passable path connecting line segments are not intersected, indicating that no road or obstacle exists between the adjacent path points, carrying out point supplementing processing, if the passable path connecting line segments are intersected, indicating that the adjacent path points have roads, planning a first path according to the passable path point set, and deleting the passable path points in the first path from the passable path point set in order to avoid path repetition. Therefore, when the trafficable path exists in the known driving area, the shortest planning route can be obtained by combining the designated path point set.
The foregoing description is only illustrative of the present invention and is not intended to limit the scope of the invention, and all equivalent changes made by the specification and drawings of the present invention, or direct or indirect application in the relevant art, are included in the scope of the present invention.