Route planning method
By generating the extended hindering zone polygon and replacing the segment with its boundaries, the problems of high computational complexity and dependence on manual intervention in existing route planning are solved, and efficient and safe route planning is achieved.
Patent Information
- Application Number
- CN202510616794.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-05-14
- Publication Date
- 2025-09-02
AI Technical Summary
The existing route planning methods have problems of excessive dependence on manual intervention and high computational complexity in large-range route planning, which is difficult to meet the needs of real-time planning.
By generating the extended hindering area polygons, the boundaries of the hindering area polygons are used to replace the sections, and obstacle avoidance routes are generated by judging whether the straight line segments intersect. The method of directly generating a straight line route is adopted to avoid the introduction of additional key points and simplify the route planning process.
It improves the algorithm efficiency of route planning, the generated route is shorter and has high safety, reduces the computational complexity, and is suitable for real-time planning.
Smart Images
Figure CN120576752A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of transportation engineering, and in particular relates to a route planning method. Background Art
[0002] Efficient route planning is crucial in modern transportation, including aviation, maritime, and land transport. With the development of intelligent ocean shipping systems and the introduction of long-range unmanned aerial vehicles, the need for efficient long-haul route planning methods is becoming increasingly urgent.
[0003] Currently, commonly used route planning methods can be divided into two main categories: sub-segment connection methods and grid-based search methods. Sub-segment connection methods generally first select a certain number of sub-segments of equal or unequal lengths, and then further obtain the route composed of sub-segments, such as the visibility graph method. Grid-based search algorithms grid the possible navigation area and then use the A* algorithm, shortest path search algorithm, etc. to determine the optimal route. Both methods are applied to long-range route planning, and they suffer from the problems of excessive reliance on human intervention and high computational complexity, respectively. For example, the visibility graph method requires manual determination of a certain number of key points in advance, which is overly dependent on human intervention; while the grid-based search algorithm requires a relatively fine-grained grid to obtain a more reasonable route. As the planned distance increases, the computational complexity increases exponentially, resulting in excessively long route planning times and difficulty meeting the route planning requirements in scenarios such as real-time planning. Summary of the Invention
[0004] The technical problem solved by the present invention is to overcome the deficiencies of the prior art and provide a route planning method with the advantages of high algorithm efficiency and short distance of the generated route.
[0005] The object of the present invention is achieved through the following technical solutions: A route planning method, comprising: obtaining an obstruction area polygon according to a preset obstruction area range; expanding the obstruction area polygon to obtain an expanded obstruction area polygon; obtaining an initial route according to a preset initial point position of the route and a preset destination point position of the route, obtaining an obstacle avoidance route according to the initial route and the expanded obstruction area polygon; and straightening the obstacle avoidance route to obtain a planned route.
[0006] In the above route planning method, in the obstruction area polygon, the vertex sequence of the obstruction area polygon is arranged counterclockwise; and each obstruction area polygon does not overlap with each other.
[0007] In the above-mentioned route planning method, expanding the obstruction area polygon to obtain the expanded obstruction area polygon includes: moving each vertex of the obstruction area polygon outward by a preset distance along the angle bisector of the polygon's vertex angle to obtain the expanded obstruction area polygon; wherein the preset distance satisfies that different sides of each expanded obstruction area polygon do not intersect with each other.
[0008] In the above-mentioned route planning method, obtaining the obstacle avoidance route based on the initial route and the expanded obstruction polygon includes: determining all segments of the initial route that are within the expanded obstruction polygon; for each segment within the expanded obstruction polygon, replacing the segment with the shorter of two paths along the boundary of the expanded obstruction polygon, ultimately obtaining a route that does not pass through the expanded obstruction polygon, namely the obstacle avoidance route.
[0009] In the above-mentioned route planning method, straightening includes: taking two adjacent segments of the obstacle avoidance route, namely the first segment and the second segment, and determining whether the line connecting the starting point of the first segment and the end point of the second segment intersects with each expanded obstacle area polygon; if they do not intersect, then using the line connecting the starting point of the first segment and the end point of the second segment as a new segment to replace the first segment and the second segment.
[0010] In the above-mentioned route planning method, determining all segments of the initial route that are within the expanded obstruction polygon includes: determining whether the initial route and the boundary line segment S of the expanded obstruction polygon Z intersect with each other; if so, obtaining a set K of all intersection points between the initial route and the boundary line segment S; and sorting all the intersection points in the set K in a direction from the starting point of the initial route to the end point of the initial route, and determining all segments of the initial route that are within the expanded obstruction polygon Z in pairs.
[0011] In the above route planning method, whether the initial route and the boundary line segment S intersect is determined by determining whether two straight line segments intersect.
[0012] In the above-mentioned route planning method, the method for determining whether two straight line segments intersect includes: the coordinates of point P1 of the straight line segment P1P2 are known to be (x1, y1), the coordinates of point P2 of the straight line segment P1P2 are known to be (x2, y2), the coordinates of point P3 of the straight line segment P3P4 are known to be (x3, y3), and the coordinates of point P4 of the straight line segment P3P4 are known to be (x4, y4); according to the coordinates of point P1 of the straight line segment P1P2, the coordinates of point P2 of the straight line segment P1P2, the coordinates of point P3 of the straight line segment P3P4, and the coordinates of point P4 of the straight line segment P3P4, the first determinant A and the second determinant A are obtained. t and the third determinant A u , if the first determinant A is zero, then the straight line segment P1P2 and the straight line segment P3P4 do not intersect; according to the first determinant A and the second determinant At Get the first determinant ratio t, according to the first determinant A and the third determinant A u Obtain the second determinant ratio u; if 0<t≤1 and 0<u≤1, then determine that the straight line segment P1P2 and the straight line segment P3P4 intersect.
[0013] In the above route planning method, the first determinant A is obtained by the following formula:
[0014]
[0015] The second determinant A t It is obtained by the following formula:
[0016]
[0017] The third determinant A u It is obtained by the following formula:
[0018]
[0019] The ratio of the first determinant t and the ratio of the second determinant u are obtained by the following formula:
[0020]
[0021] The coordinates of the intersection of the straight line segment P1P2 and the straight line segment P3P4 are obtained by the following formula:
[0022]
[0023] Wherein, x is the abscissa of the intersection of the straight line segment P1P2 and the straight line segment P3P4, and y is the ordinate of the intersection of the straight line segment P1P2 and the straight line segment P3P4.
[0024] A route planning system includes: a first module for obtaining an obstruction area polygon based on a preset obstruction area range; a second module for expanding the obstruction area polygon to obtain an expanded obstruction area polygon; a third module for obtaining an initial route based on a preset route initial point position and a preset route destination point position, and obtaining an obstacle avoidance route based on the initial route and the expanded obstruction area polygon; and a fourth module for straightening the obstacle avoidance route to obtain a planned route.
[0025] Compared with the prior art, the present invention has the following beneficial effects:
[0026] (1) The present invention uses the expanded obstruction zone boundary as the endpoint of the straight segment, without introducing additional key points for determining the sub-segment set, and has strong operability;
[0027] (2) The present invention can maximize the safety of the route by increasing the extension distance of the obstruction area;
[0028] (3) The present invention generates a route by directly generating a straight route, then replacing the route segments within the obstruction area with the obstruction area or the expanded boundary of the obstruction area, and then straightening the route as much as possible. The algorithm is highly efficient and can ensure that the distance of the generated route is short. BRIEF DESCRIPTION OF THE DRAWINGS
[0029] Various other advantages and benefits will become apparent to those skilled in the art upon reading the detailed description of the preferred embodiment below. The accompanying drawings are for illustration purposes only and are not to be considered as limiting the present invention. The same reference symbols are used throughout the drawings to represent the same components. In the drawings:
[0030] Figure 1 This is a schematic diagram of route planning input provided by an embodiment of the present invention;
[0031] Figure 2 This is a result diagram of the expansion of the obstruction area provided by an embodiment of the present invention;
[0032] Figure 3 is a schematic diagram of the obstacle avoidance route generation result provided by an embodiment of the present invention;
[0033] Figure 4 Schematic diagram of a route straightening process provided by an embodiment of the present invention. DETAILED DESCRIPTION
[0034] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art. It should be noted that, unless there is a conflict, the embodiments of the present disclosure and the features described in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0035] This embodiment provides a route planning method, which includes: obtaining an obstruction area polygon based on a preset obstruction area range; expanding the obstruction area polygon to obtain an expanded obstruction area polygon; obtaining an initial route based on a preset route initial point position and a preset route destination point position, obtaining an obstacle avoidance route based on the initial route and the expanded obstruction area polygon; and straightening the obstacle avoidance route to obtain a planned route.
[0036] In the obstruction area polygon, the vertex sequence of the obstruction area polygon is arranged counterclockwise; each obstruction area polygon does not overlap with each other.
[0037] Expanding the obstruction polygon to obtain the expanded obstruction polygon includes: moving each vertex of the obstruction polygon outward by a preset distance along the angle bisector of the vertex angle of the polygon to obtain the expanded obstruction polygon; wherein the preset distance satisfies that different sides of each expanded obstruction polygon do not intersect each other.
[0038] Obtaining an obstacle avoidance route based on the initial route and the expanded obstruction polygon includes: determining all segments of the initial route within the expanded obstruction polygon; for each segment within the expanded obstruction polygon, replacing the segment with the shorter of two paths along the boundary of the expanded obstruction polygon, and ultimately obtaining a route that does not pass through the expanded obstruction polygon, namely the obstacle avoidance route.
[0039] The straightening process includes: taking two adjacent segments of the obstacle avoidance route, namely the first segment and the second segment, and determining whether a line connecting the starting point of the first segment and the end point of the second segment intersects with each expanded obstacle area polygon; if neither intersects, taking a line connecting the starting point of the first segment and the end point of the second segment as a new segment to replace the first segment and the second segment.
[0040] Determining all flight segments of the initial route that are within the expanded navigation obstruction polygon includes: determining, for a boundary line segment S of the expanded navigation obstruction polygon Z, whether the initial route and the boundary line segment S intersect; if so, obtaining a set K of all intersection points of the initial route and the boundary line segment S; and sorting all the intersection points in the set K in a direction from the starting point of the initial route to the end point of the initial route, and determining all flight segments of the initial route that are within the expanded navigation obstruction polygon Z in pairs.
[0041] Whether the initial route and the boundary line segment S intersect is determined by judging whether two straight line segments intersect.
[0042] The method for determining whether two straight line segments intersect includes: knowing that the coordinates of point P1 of straight line segment P1P2 are (x1, y1), knowing that the coordinates of point P2 of straight line segment P1P2 are (x2, y2), knowing that the coordinates of point P3 of straight line segment P3P4 are (x3, y3), and knowing that the coordinates of point P4 of straight line segment P3P4 are (x4, y4); obtaining the first determinant A and the second determinant A according to the coordinates of point P1 of straight line segment P1P2, the coordinates of point P2 of straight line segment P1P2, the coordinates of point P3 of straight line segment P3P4, and the coordinates of point P4 of straight line segment P3P4. t and the third determinant A u , if the first determinant A is zero, then the straight line segment P1P2 and the straight line segment P3P4 do not intersect; according to the first determinant A and the second determinant A tGet the first determinant ratio t, according to the first determinant A and the third determinant A u Obtain the second determinant ratio u; if 0<t≤1 and 0<u≤1, then determine that the straight line segment P1P2 and the straight line segment P3P4 intersect.
[0043] The first determinant A is obtained by the following formula:
[0044]
[0045] The second determinant A t It is obtained by the following formula:
[0046]
[0047] The third determinant A u It is obtained by the following formula:
[0048]
[0049] The ratio of the first determinant t and the ratio of the second determinant u are obtained by the following formula:
[0050]
[0051] The coordinates of the intersection of the straight line segment P1P2 and the straight line segment P3P4 are obtained by the following formula:
[0052]
[0053] Wherein, x is the abscissa of the intersection of the straight line segment P1P2 and the straight line segment P3P4, and y is the ordinate of the intersection of the straight line segment P1P2 and the straight line segment P3P4.
[0054] Specifically, the method includes the following steps:
[0055] Step 1: Parameter input.
[0056] Input the preset route initial point position, preset route destination point position and preset navigation area range. The navigation area range is represented by a number of navigation area polygons. Each navigation area polygon is represented by a counterclockwise sequence of polygon vertices, such as Figure 1 Without loss of generality, it is limited that the obstruction polygons do not overlap with each other.
[0057] Step 2: Expand each obstruction polygon in step 1 to obtain an expanded obstruction polygon.
[0058] Take an appropriate distance value d, move each vertex of each obstruction polygon outward along the angle bisector of the polygon vertex, and obtain the expanded obstruction polygon, as shown in the following example: Figure 2For simplicity, the extended distance d is limited to ensure that the different sides of the extended obstruction areas do not intersect with each other.
[0059] Step 3: Generate obstacle avoidance route.
[0060] First, connect the initial point position in step 1 with the destination point position to obtain an initial route; then determine all the segments of the initial route within each expanded obstruction polygon. For each segment B within the expanded obstruction polygon, replace segment B with the shorter of the two paths along the boundary of the expanded obstruction polygon. Finally, a route that does not pass through the expanded obstruction polygon is obtained, such as Figure 3 The specific plan is as follows:
[0061] M1: Initialize the route as a straight line from the starting point to the destination point;
[0062] M2: Perform operations from M3 to M5 for each obstruction area Z;
[0063] M3: For each segment L in the route, perform operations from M4 to M5;
[0064] M4: Determine whether the flight segment L passes through the obstruction area Z. If so, execute M5.
[0065] M5: For each segment B of segment L passing through the obstruction area Z, calculate the lengths of the two paths from the starting point of segment B along the boundary of Z to the end point of segment B, and replace segment B in segment L with the shorter path.
[0066] The above steps require the use of the "Technical Solution for Determining Whether a Segment Passes Through an Obstruction Area and Identifying the Segment Located Within an Obstruction Area", and the "Technical Solution for Determining Whether Two Line Segments Intersect and Determining the Intersection Point" requires the use of the "Technical Solution for Determining Whether Two Line Segments Intersect and Determining the Intersection Point", which are as follows:
[0067] (1) Technical solution for determining whether a flight segment passes through an obstruction area and determining the flight segment located within an obstruction area
[0068] The method for determining whether the flight segment L passes through the interior of the obstruction area Z and determining whether the flight segment L is within the interior of the obstruction area Z is as follows:
[0069] M1: For each boundary line segment S of Z, use the "Technical Solution for Determining Whether Two Line Segments Intersect and Determining Intersection Points" to determine whether line segment L and line segment S intersect and determine the intersection point, and obtain the set K of all intersection points between L and the boundary line segment of Z;
[0070] M2: Sort all the intersection points in K in the direction from the starting point of L to the end point of L, and determine the set Q of segments of L inside Z in pairs (the first point is the starting point of the segment, and the second point is the end point of the segment).
[0071] (2) Technical solution for determining whether two straight line segments intersect and the intersection point
[0072] Mark P i The coordinates of (x i ,y i ), i = 1, 2, 3, 4; the coordinates of point P are (x, y). The algorithm for determining whether two straight line segments P1P2 and P3P4 intersect and finding the coordinates of the intersection point P is as follows:
[0073] M1: Calculate the determinant A
[0074]
[0075] If the determinant A is zero, then the line segments P1P2 and P3P4 are determined to be disjoint, and the result is returned;
[0076] M2: Calculate the determinant, A t and A u They are all determinants.
[0077]
[0078] and parameters
[0079]
[0080] If 0 < t ≤ 1 and 0 < u ≤ 1, the two line segments are determined to intersect, and the coordinates of the intersection are calculated using the following formula:
[0081]
[0082] Where t is the ratio of the first determinant and u is the ratio of the second determinant.
[0083] Otherwise, the two line segments are determined to be non-intersecting.
[0084] Step 4: Straighten the route
[0085] Take two adjacent flight segments and determine whether the line connecting the start point of the first segment and the end point of the second segment intersects with each of the expanded obstruction polygons. If neither intersects, replace the first and second segments with the line connecting the start point of the first segment and the end point of the second segment. Repeat the above steps until all replacements are completed.
[0086] Try to use straight lines that do not pass through the obstruction area to replace the broken lines in the route to obtain route values. The specific plan for straightening the route is as follows:
[0087] M1: Set the end flag bend to 0;
[0088] M2: If the end flag bend is 0, execute M3 to M7;
[0089] M3: Set the end flag bend to 1, and perform operations M4 to M7 on every two adjacent segments P(i)P(i+1) and P(i+1)P(i+2) in the route from front to back;
[0090] M4: Set the two-segment straightening flag bkezhi to 1, and perform M5 for each obstruction area Z;
[0091] M5: Determine whether the line segment P(i)P(i+2) passes through the obstruction zone Z. If so, set bkezhi to 0 and go to M6.
[0092] M6: If bkezhi is 1, replace the two segments P(i)P(i+1) and P(i+1)P(i+2) in the original route with the segment P(i)P(i+2), and set bend to 0;
[0093] M7: Convert to M2.
[0094] The route changes during the route straightening process using the above route straightening algorithm are as follows: Figure 4 As shown in the 5 sub-figures in .
[0095] This embodiment also provides a route planning system, which includes: a first module for obtaining an obstruction area polygon based on a preset obstruction area range; a second module for expanding the obstruction area polygon to obtain an expanded obstruction area polygon; a third module for obtaining an initial route based on a preset initial point position of the route and a preset destination point position of the route, and obtaining an obstacle avoidance route based on the initial route and the expanded obstruction area polygon; and a fourth module for straightening the obstacle avoidance route to obtain a planned route.
[0096] This embodiment uses the expanded obstruction area boundary as the endpoint of the straight line segment, eliminating the need to introduce additional key points for determining the sub-segment set, and thus has strong operability. This embodiment can maximize the safety of the route by increasing the obstruction area expansion distance. This embodiment generates a route by directly generating a straight line route, then replacing the segments within the obstruction area with the obstruction area or the expanded obstruction area boundary, and then straightening the route as much as possible. This algorithm is highly efficient and can ensure that the distance of the generated route is short.
[0097] Although the present invention has been disclosed above in terms of preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art may make possible changes and modifications to the technical solutions of the present invention by using the methods and technical contents disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the scope of protection of the technical solutions of the present invention.
Claims
1. A route planning method, characterized in that include: Obtaining the obstruction area polygon according to the preset obstruction area range; Expanding the obstruction area polygon to obtain an expanded obstruction area polygon; An initial route is obtained according to the preset route initial point position and the preset route destination point position, and an obstacle avoidance route is obtained according to the initial route and the expanded obstruction area polygon; The obstacle avoidance route is straightened to obtain the planned route.
2. The route planning method according to claim 1, wherein: In the obstruction area polygon, the vertex sequence of the obstruction area polygon is arranged counterclockwise; each obstruction area polygon does not overlap with each other.
3. The route planning method according to claim 1, wherein: The expanded obstruction area polygon includes: Each vertex of the obstruction polygon is moved outward by a preset distance along the angle bisector of the polygon vertex to obtain an expanded obstruction polygon; wherein the preset distance satisfies that different sides of each expanded obstruction polygon do not intersect each other.
4. The route planning method according to claim 1, wherein: Obstacle avoidance routes obtained based on the initial route and the expanded obstruction area polygon include: Determine all segments of the initial route that are within the expanded obstruction polygon; For each flight segment within the expanded obstruction polygon, the shorter of the two paths along the boundary of the expanded obstruction polygon is used to replace the flight segment, and finally a route that does not pass through the expanded obstruction polygon is obtained, namely the obstacle avoidance route.
5. The route planning method according to claim 1, wherein: Straightening includes: Take two adjacent segments of the obstacle avoidance route, namely the first segment and the second segment, and determine whether the line connecting the starting point of the first segment and the end point of the second segment intersects with each expanded obstacle area polygon. If they do not intersect, the line connecting the starting point of the first segment and the end point of the second segment is used as a new segment to replace the first segment and the second segment.
6. The route planning method according to claim 4, characterized in that: All segments of the initial route that are determined to be within the expanded obstruction polygon include: For the boundary line segment S of the expanded obstruction area polygon Z, determine whether the initial route and the boundary line segment S intersect. If they do intersect, obtain a set K of all intersection points between the initial route and the boundary line segment S. All intersection points in the set K are sorted in the direction from the starting point to the end point of the initial route, and all segments of the initial route located inside the expanded obstruction polygon Z are determined in pairs.
7. The route planning method according to claim 6, characterized in that: Whether the initial route and the boundary line segment S intersect is determined by judging whether two straight line segments intersect.
8. The route planning method according to claim 7, characterized in that: Methods for determining whether two straight line segments intersect include: The coordinates of point P1 on the line segment P1P2 are known to be (x1, y1), the coordinates of point P2 on the line segment P1P2 are known to be (x2, y2), the coordinates of point P3 on the line segment P3P4 are known to be (x3, y3), and the coordinates of point P4 on the line segment P3P4 are known to be (x4, y4); According to the coordinates of point P1 of the straight line segment P1P2, the coordinates of point P2 of the straight line segment P1P2, the coordinates of point P3 of the straight line segment P3P4 and the coordinates of point P4 of the straight line segment P3P4, the first determinant A and the second determinant A are obtained. t and the third determinant A u , if the first determinant A is zero, then the straight line segment P1P2 and the straight line segment P3P4 do not intersect; According to the first determinant A and the second determinant A t Get the first determinant ratio t, according to the first determinant A and the third determinant A u Get the second determinant ratio u; If 0<t≤1 and 0<u≤1, it is determined that the straight line segment P1P2 and the straight line segment P3P4 intersect.
9. The route planning method according to claim 8, characterized in that: The first determinant A is obtained by the following formula: The second determinant A t It is obtained by the following formula: The third determinant A u It is obtained by the following formula: The ratio of the first determinant t and the ratio of the second determinant u are obtained by the following formula: The coordinates of the intersection of the straight line segment P1P2 and the straight line segment P3P4 are obtained by the following formula: Wherein, x is the abscissa of the intersection of the straight line segment P1P2 and the straight line segment P3P4, and y is the ordinate of the intersection of the straight line segment P1P2 and the straight line segment P3P4.
10. A route planning system, characterized in that include: The first module is used to obtain the obstruction area polygon according to the preset obstruction area range; The second module is used to expand the obstruction area polygon to obtain an expanded obstruction area polygon; The third module is used to obtain an initial route according to the preset route initial point position and the preset route destination point position, and obtain an obstacle avoidance route according to the initial route and the expanded obstruction area polygon; The fourth module is used to straighten the obstacle avoidance route to obtain the planned route.