Route planning system and method fusing steering cost

Abstract

The invention relates to a steering cost fused path planning system and method, and belongs to the technical field of path planning. Comprising the following steps: S1, constructing a path planning graph; s2, determining a starting point and an ending point of a path; s3, defining a state in the priority queue; s4, judging whether the priority queue is empty or not; s5, judging whether the current node corresponding to the state with the minimum current accumulated cost is an end point or not; s6, traversing a next neighbor node of the current node, and calculating a steering cost between the current node and the next neighbor node based on the precursor node; s7, calculating the total cost from the starting point to the next neighbor node; and S8, when the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, updating the total cost from the starting point to the next neighbor node to the priority queue. The distance and steering cost are integrated in path planning, the path is smoother through a large-punishment steering angle, and the method has more advantages in the aspects of actual passing time, energy consumption and mechanical loss.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a path planning system and method that integrates turning costs. Background Technology

[0002] Currently, classic path planning algorithms, such as Dijkstra's algorithm, mainly focus on the cumulative travel distance or time of a path. They typically abstract the path network as a graph structure, where nodes represent intersections, edges represent road segments, and the weight of the edges represents the length of the road segment or the travel time.

[0003] However, in practical applications, distance alone is insufficient. For example, in autonomous driving and robot navigation, frequent and excessively sharp turns can lead to numerous problems: reduced traffic efficiency: sharp turns require deceleration, increasing overall travel time; increased mechanical wear: causing additional wear on the steering system of the vehicle or robot; impact on comfort and safety: sharp turns can affect passenger comfort and pose safety risks in certain scenarios (such as high speeds and slippery surfaces); increased energy consumption: frequent acceleration, deceleration, and turns lead to increased energy consumption.

[0004] While some existing path planning technologies consider turning restrictions (such as prohibiting left turns), they are typically based on rule-based, discrete processing methods (e.g., directly prohibiting certain turns), lacking fine-grained quantification of the cost of turning continuity. How to smoothly and quantitatively integrate continuous turning angles into the path cost model to generate more natural, efficient, and low-loss paths is a pressing problem in this field. Summary of the Invention

[0005] The technical problem to be solved by the present invention is: in order to overcome at least one technical problem existing in the prior art, the present invention provides a path planning system and method that integrates turning costs.

[0006] The technical solution adopted by this invention to solve its technical problem is: This invention provides a path planning method that integrates turning costs, comprising the following steps: S1. Construct a path planning graph including nodes and edges, and assign an edge weight value to each edge; S2. In response to the user's route planning request, determine the start and end points of the route based on the route planning map; S3. Define the state in the priority queue, which includes the current cumulative cost, the current node, and the predecessor node; S4. Determine if the priority queue is empty. If not, pop the state with the lowest current cumulative cost in the priority queue and jump to step S5. If yes, the path planning ends. S5. Determine whether the current node in the state with the minimum cumulative cost is the destination. If yes, output the path from the starting point to the current node and the corresponding current cumulative cost. If no, jump to step S6. S6. Traverse the next neighbor node of the current node and calculate the turning cost between the current node and the next neighbor node based on the predecessor node. S7. Based on the turning cost, edge weight value, and current cumulative cost, calculate the total cost from the starting point to the next neighbor node; S8. When the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, the total cost from the starting point to the next neighbor node is updated to the priority queue, and then the process jumps to step S4.

[0007] Furthermore, the edge is a path segment connecting the node, and the edge weight value is the distance or travel time of each edge.

[0008] Furthermore, step S5 includes: S51. Define an identifier for the node. Determine whether the identifier of the current node corresponding to the current state with the minimum cumulative cost is the same as the identifier of the endpoint. If yes, the current node corresponding to the current state with the minimum cumulative cost is the endpoint. Then jump to step S52. If no, jump to step S6. S52. Define a parent node dictionary, backtrack based on the parent node dictionary and output the path from the starting point to the current node and the corresponding current cumulative cost.

[0009] Furthermore, step S6 includes: S61. Determine if the current node has a corresponding predecessor node. If not, the turning cost between the current node and the next neighbor node is 0. If yes, jump to step S62. S62. Define the vector from the predecessor node to the current node as the predecessor vector, and the vector from the current node to the next neighbor node as the current vector. S63. Based on the angle between the predecessor vector and the current vector, calculate the turning cost between the current node and the next neighbor node.

[0010] Furthermore, the method for calculating the angle between the predecessor vector and the current vector includes: S631. Calculate the dot product and magnitude based on the predecessor vector and the current vector; S632. Based on the dot product and magnitude, and using the inverse cosine function, calculate the angle between the predecessor vector and the current vector.

[0011] Furthermore, the formula for calculating the steering cost is a linear function, and the linear function is: ; In the formula, It is a linear function of the turning cost; The angle between the predecessor vector and the current vector; These are the weighting coefficients.

[0012] Furthermore, the formula for calculating the total cost from the starting point to the next neighbor node is as follows: Total cost = current cumulative cost + edge weight value + turning cost.

[0013] Furthermore, in step S8, the total cost from the starting point to the next neighbor node is updated in the priority queue, including: Create a new state based on the total cost from the starting point to the next neighbor node, and update the new state to the priority queue.

[0014] Secondly, the present invention also provides a path planning method that integrates turning costs, wherein the formula for calculating the turning costs is a nonlinear function, and the nonlinear function is: ; In the formula, For the nonlinear function of the turning cost; The angle between the predecessor vector and the current vector; These are the weighting coefficients.

[0015] Thirdly, the present invention also provides a path planning system that integrates turning costs, comprising: The graph model building module is used to construct a path planning graph that includes a set of nodes and a set of edges. The request receiving module is used to receive users' route planning requests; The path planning module is used to define and manage the states in the priority queue, calculate the turning cost between the current node and the next neighbor node based on the states in the priority queue, calculate the total cost from the starting point to the next neighbor node based on the turning cost, update the total cost from the starting point to the next neighbor node to the priority queue when the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, and output the path from the starting point to the current node and the corresponding current cumulative cost when the current node corresponding to the state with the minimum current cumulative cost is the destination.

[0016] Fourthly, the present invention also provides an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program, when executed by the processor, implements the path planning method for fusion turning costs described above. Fifthly, the present invention also provides a readable storage medium that, when the instructions in the storage medium are executed by a processor of an electronic device, enables the electronic device to execute the aforementioned path planning method for fusion turning costs.

[0017] The beneficial effects of this invention are: 1. Smoother and more natural paths: By penalizing large steering angles, the algorithm tends to choose straight or small-angle turns, avoiding unnecessary "zigzag" paths and generating paths that are more in line with human driving or robot movement habits.

[0018] 2. Lower overall cost: Although the planned route may not be the shortest in terms of pure distance, it takes into account distance and turning costs, and is often more advantageous in terms of actual travel time, energy consumption and mechanical wear.

[0019] 3. Strong algorithm versatility: This invention is an improvement on the classic Dijkstra algorithm, which is easy to understand and implement. It can be seamlessly integrated into existing path planning frameworks and can be used in combination with other optimization strategies (such as the heuristic function of A*).

[0020] 4. Highly configurable: The steering cost function can be flexibly defined to adapt to different application scenarios. Attached Figure Description

[0021] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0022] Figure 1 This is a flowchart of the path planning method for fusion turning cost provided in Embodiment 1 of the present invention; Figure 2 This is a flowchart of step S5 provided in Embodiment 1 of the present invention; Figure 3 A flowchart of step S6 provided in Embodiment 1 of the present invention; Figure 4 A flowchart of step S631 provided in Embodiment 1 of the present invention; Figure 5 This is a path planning diagram provided in Embodiment 1 of the present invention; Figure 6 This is a schematic diagram of the path planning system with fused turning costs provided in Embodiment 3 of the present invention. Figure 7 This is a partial block diagram of an electronic device provided in Embodiment 4 of the present invention. Detailed Implementation

[0023] Before discussing the exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although the flowcharts describe the operations as sequential processes, many of these operations can be performed in parallel, concurrently, or simultaneously. Furthermore, the order of the operations can be rearranged. The process can be terminated when its operation is completed, but may also have additional steps not included in the figures. The process can correspond to a method, function, procedure, subroutine, subroutine, etc.

[0024] It should be understood that although the terms "first," "second," etc., may be used herein to describe various units, these units should not be limited by these terms. These terms are used merely to distinguish one unit from another. For example, without departing from the scope of the exemplary embodiments, a first unit may be referred to as a second unit, and similarly, a second unit may be referred to as a first unit. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0025] The present invention will now be described in detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0026] For ease of understanding, the inventive concept will be described in its entirety before a detailed description of the embodiments of the present invention: This invention provides a path planning system and method that integrates steering costs, aiming to quantify the physical costs of steering actions (such as time and energy consumption) as part of the path weights, thereby planning a path with a better total cost (distance cost + steering cost). First, the state definition of the priority queue is expanded by introducing the predecessor node as a key dimension of the state, in addition to recording the current node and the current accumulated cost. This lays the foundation for the continuity of path direction tracking and the quantification of steering behavior. Second, a dynamic steering cost quantification and integration mechanism is created. Vector geometric operations are used to accurately calculate the angle between the current vector and the predecessor vector, and a configurable cost function is used to map this continuous steering angle into a cost value. Finally, the traditional path cost model is innovated from a single accumulation of edge weights to a comprehensive cost model that integrates edge weight values ​​and steering costs.

[0027] Example 1 like Figure 1 As shown, this embodiment provides a path planning method that integrates turning costs, including: S1. Construct a path planning graph that includes nodes and edges, and assign an edge weight value to each edge. Nodes include, but are not limited to, the start point, the end point, the current node, the predecessor node, and the next neighbor node. Edges are path segments connecting nodes, and the edge weight value is the distance or basic travel time of each edge.

[0028] S2. In response to the user's route planning request, determine the start and end points of the route based on the route planning map.

[0029] S3. Define the state in the priority queue. The state includes the current cumulative cost, the current node, and the predecessor node. The state is represented as (current cumulative cost, current node, predecessor node). The current cumulative cost represents the cumulative cost from the starting point to the current node. In this embodiment, the predecessor node is the node before the current node. By introducing the predecessor node, the direction of the path is recorded, providing the necessary historical information for calculating the turning angle.

[0030] S4. Determine if the priority queue is empty. If not, pop the state with the lowest current cumulative cost in the priority queue and jump to step S5. If yes, the path planning ends.

[0031] Specifically, popping the state with the lowest current cumulative cost from the priority queue is a normal characteristic of the priority queue. When the priority queue is empty, it means that all nodes in the path have been traversed, and the path planning ends.

[0032] S5. Determine whether the current node in the state with the minimum cumulative cost is the destination. If yes, output the path from the starting point to the current node and the corresponding current cumulative cost. If not, jump to step S6.

[0033] In some feasible implementations, combined with Figure 2 As shown, step S5 includes: S51. Define an identifier for the node. Determine whether the identifier of the current node corresponding to the current state with the minimum cumulative cost is the same as the identifier of the endpoint. If yes, the current node corresponding to the current state with the minimum cumulative cost is the endpoint. Then jump to step S52. If no, jump to step S6. S52. Define a parent node dictionary, backtrack based on the parent node dictionary and output the path from the starting point to the current node and the corresponding current cumulative cost.

[0034] S6. Traverse the next neighbor node of the current node and calculate the turning cost between the current node and the next neighbor node based on the predecessor node.

[0035] In some feasible implementations, combined with Figure 3 As shown, step S6 includes: S61. Determine if the current node has a corresponding predecessor node. If not, the turning cost between the current node and its next neighbor node is 0. If yes, proceed to step S62. Specifically, when starting from the origin, the current node is the origin, and there is no predecessor node, so the turning cost is 0. S62. Define the vector from the predecessor node to the current node as the predecessor vector, and the vector from the current node to the next neighbor node as the current vector. Specifically, if the predecessor node is... The current node is The next neighbor node is Then the predecessor vector is The current vector is .

[0036] S63. Based on the angle between the predecessor vector and the current vector, calculate the turning cost between the current node and the next neighbor node.

[0037] In some feasible implementations, combined with Figure 4 As shown, the method for calculating the angle between the predecessor vector and the current vector includes: S631. Calculate the dot product and magnitude based on the predecessor vector and the current vector. Specifically, the dot product... The calculation formula is: The magnitude of the precursor vector for The magnitude of the current vector for .

[0038] S632. Based on the dot product and magnitude, calculate the cosine of the angle between the predecessor vector and the current vector, and then use the inverse cosine function to calculate the angle between the predecessor vector and the current vector. Specifically, In the formula, The angle between the predecessor vector and the current vector. The dot product of the predecessor vector and the current vector. Let be the magnitude of the predecessor vector. Given the magnitude of the current vector, calculate Then, the angle between the predecessor vector and the current vector can be obtained using the inverse cosine function math.acos. .

[0039] It's important to note that the dot product of two vectors geometrically measures their degree of coherence in the same direction. When normalized using the magnitudes of the vectors, the result equals the cosine of the angle between the two vectors. Simply put, if the predecessor and current vectors are in the exact same direction, the dot product equals the product of their magnitudes. , The angle is 0°; if the predecessor vector and the current vector are perpendicular, the dot product is 0. , With a value of 90°, this formula precisely connects the abstract algebraic operation (dot product) with the intuitive geometric concept (angle).

[0040] In some feasible implementations, the steering cost is calculated using a linear function, which is: ; In the formula, It is a linear function of the turning cost; The angle between the predecessor vector and the current vector; The weighting coefficient is 0.5, which is preferred in this embodiment.

[0041] It should be noted that regarding the included angle The absolute value is used because steering is divided into left and right steering, and in our use case, we only need to care about the magnitude of the steering angle, not the direction of steering. Therefore, the angle is calculated as an absolute value, multiplied by a weighting coefficient. This is to balance the weight between straight-line travel and steering angle. When the linear speed is high and the angular velocity is low, it means that straight-line travel takes much less time than steering. The value of K can be increased, thereby significantly increasing the cost of steering actions in the total path cost and imposing a stronger penalty on steering (especially large-angle steering), thus generating a path with a higher proportion of straight-line travel and fewer steering operations. When the linear speed is low and the angular velocity is high, it means that straight-line travel takes much more time than steering. The value of K can be decreased (it can even be negative) to achieve the goal of minimizing or eliminating penalties for steering.

[0042] It is worth mentioning that when using a linear cost function, the penalty intensity and the steering angle always maintain a strictly linear proportional relationship, resulting in a uniform increase in cost with the angle. For example, the linear cost function of this embodiment can be applied to a two-wheel differential chassis. Because the motion model of a two-wheel differential chassis allows for in-place steering, the steering process is essentially continuous, and its control difficulty and energy consumption are approximately linearly related to the steering angle. Therefore, using a linear cost function to apply a gentle and uniform penalty aims to optimize steering efficiency and smoothness, without completely eliminating large-angle steering.

[0043] S7. Based on the turning cost, edge weights, and current cumulative cost, calculate the total cost from the starting point to the next neighbor node. The formula for calculating the total cost from the starting point to the next neighbor node is as follows: Total cost = Current cumulative cost + Edge weights + Turning cost; Current cumulative cost represents the cumulative cost from the starting point to the current node, and edge weights represent the distance or basic travel time of each edge.

[0044] It should be noted that using addition to fuse turning costs ensures that the total cost from the starting point to the end point is equal to the sum of the basic toll costs of each path segment and the turning operation costs at each node. This not only meets the basic requirements of Dijkstra and other algorithms for weight accumulation, but also truly reflects the physical nature of the linear superposition of costs such as energy consumption, time, and mechanical wear caused by travel distance and turning operation in actual applications.

[0045] It should be noted that this algorithm effectively solves the problem of traditional methods that only optimize distance while ignoring the physical cost of steering by quantifying the steering angle as a cost and incorporating it into the cost model of classic path planning algorithms. The generated path is significantly smoother and more natural due to the penalty for large-angle steering, avoiding unnecessary jagged trajectories and better conforming to actual driving or sporty habits. Although it may not be the shortest in terms of pure distance, the total cost that takes into account the steering cost is more advantageous in terms of actual travel time, energy consumption, and mechanical wear, improving overall efficiency and economy. In addition, this improvement is implemented based on a general framework and is easy to integrate into existing systems.

[0046] S8. When the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, the total cost from the starting point to the next neighbor node is updated to the priority queue, and then the process jumps to step S4.

[0047] Specifically, the total cost from the origin to the next neighbor node (the new cost) is compared with the cost of any other previously recorded path from the origin to that next neighbor node. If the new cost is lower than all known costs to reach that next neighbor node, the new cost is updated to the "historical minimum cost from the origin to the next neighbor node". A new state is created based on the updated "historical minimum cost from the origin to the next neighbor node" and the new state is updated in the priority queue for subsequent expansion.

[0048] For example, in combination Figure 5 As shown, the node coordinates in the path planning graph are: S0(0,0), S1(1,0), S2(2,0), S3(2,1), S4(3,0), S5(3,1), S6(3,2); the edge connections and Euclidean distances are: S0-S1: =1; S1-S2: =1; S1-S3: S2-S4: S3-S5: S4-S5: S5-S6: .

[0049] Where S0(0,0) is the starting point and S6(3,2) is the ending point, the possible path analysis is as follows: Path 1: S0 → S1 → S2 → S4 → S5 → S6; Path 2: S0 → S1 → S3 → S5 → S6.

[0050] Calculation of turning cost for path 1: S0→S1: Initial segment, no turning cost (vector: (1,0)); S1→S2: The predecessor vector is (1,0), the current vector is (1,0), the angle is 0°, and the turning cost is 0; S2→S4: The predecessor vector is (1,0), the current vector is (1,0), the angle is 0°, and the steering cost is 0. S4→S5: The predecessor vector is (1,0), the current vector is (0,1), the included angle is 90° = π / 2 ≈ 1.571 radians, and the steering cost is 1.571 × 0.5 = 0.7855. S5→S6: The predecessor vector is (0,1), the current vector is (0,1), the included angle is 0°, and the steering cost is 0.

[0051] The distance cost (edge ​​weight) of path 1 is 1+1+1+1+1=5; the total cost of path 1 = distance cost + turning cost = 5+0.7855=5.7855.

[0052] Calculation of turning cost for Path 2: S0→S1: Initial segment, no turning cost (vector: (1,0)); S1→S3: The predecessor vector is (1,0), and the current vector is (1,1); Angle calculation: dot = (1×1 + 0×1) =1, |v1| = 1, |v2| = cosθ = 1 / ≈ 0.7071, θ = acos(0.7071) = 45° = π / 4 ≈0.7854 radians; turning cost = 0.7854 × 0.5 = 0.3927; S3→S5: The predecessor vector is (1,1), and the current vector is (1,0); Angle calculation: dot = (1×1 + 1×0) =1, |v1| = , |v2| = 1, cosθ = 1 / ≈ 0.7071, θ = 45° = π / 4 ≈ 0.7854 radians, turning cost = 0.7854 × 0.5 = 0.3927; S5→S6: The predecessor vector is (1,0), and the current vector is (0,1); the included angle is 90° = 1.571 radians; the steering cost is 1.571 × 0.5 = 0.7855.

[0053] The distance cost (edge ​​weight) of path 2 is 1 + 1.414 + 1 + 1 = 4.414; the total cost of path 2 = distance cost + turning cost = 4.414 + 0.3927 + 0.3927 + 0.7855 = 5.9849.

[0054] The comparison and analysis table of Path 1 and Path 2 is as follows:

[0055] Example 2 This embodiment provides a path planning method that integrates turning costs as in Embodiment 1, wherein the formula for calculating the turning cost is a nonlinear function, and the formula for the nonlinear function is: ; In the formula, For the nonlinear function of the turning cost; The angle between the predecessor vector and the current vector; The weighting coefficient is 0.5, which is preferred in this embodiment.

[0056] It should be noted that the weighting coefficients The design is the same as in Implementation Example 1. It exhibits symmetry, treating left and right turns equally, and its penalty increases quadratically with the angle. This results in a gradual increase in cost for small-angle turns, allowing for minor directional adjustments without significantly increasing costs, thus ensuring flexibility in path planning. However, for large-angle turns, the cost rises rapidly, creating a strong penalty effect. This effectively avoids sharp turns that could be dangerous, time-consuming, or cause significant damage to the steering mechanism, ultimately guiding the algorithm to generate safer, more efficient, and smoother paths that conform to the vehicle's motion characteristics. For example, the nonlinear cost function can be applied to the Ackermann chassis.

[0057] Example 3 like Figure 6 As shown, this embodiment provides a path planning system that integrates turning costs, including: The graph model building module is used to construct a path planning graph that includes a set of nodes and a set of edges. The request receiving module is used to receive users' route planning requests; The path planning module is used to define and manage the states in the priority queue, calculate the turning cost between the current node and the next neighbor node based on the states in the priority queue, calculate the total cost from the starting point to the next neighbor node based on the turning cost, update the total cost from the starting point to the next neighbor node to the priority queue when the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, and output the path from the starting point to the current node and the corresponding current cumulative cost when the current node corresponding to the state with the minimum current cumulative cost is the destination.

[0058] The functions of all the specific modules mentioned above are described in Embodiment 1, and the relevant technical details mentioned in Embodiment 1 are also effective in Embodiment 3. To avoid repetition, they will not be repeated here. Correspondingly, the relevant technical details mentioned in this embodiment can also be applied to Embodiments 1 and 2.

[0059] It is worth mentioning that all modules involved in this embodiment are logical units. In practical applications, a logical unit can be a physical unit, a part of a physical unit, or a combination of multiple physical units. Furthermore, to highlight the innovative aspects of this invention, this embodiment does not introduce units that are not closely related to solving the technical problem proposed by this invention; however, this does not mean that other units are absent from this embodiment.

[0060] Example 4 Please see Figure 7 This embodiment also provides an electronic device, including: a memory and a processor; the memory stores at least one program instruction; the processor loads and executes the at least one program instruction to implement the path planning method for fusion steering cost provided in Embodiment 1.

[0061] The memory 702 and processor 701 are connected via a bus, which may include any number of interconnecting buses and bridges, connecting various circuits of one or more processors 701 and memory 702 together. The bus may also connect various other circuits, such as peripheral devices, voltage regulators, and power management circuits, which are well known in the art and therefore will not be described further herein. A bus interface provides an interface between the bus and the transceiver. The transceiver may be a single element or multiple elements, such as multiple receivers and transmitters, providing a unit for communicating with various other devices over a transmission medium. Data processed by processor 701 is transmitted over a wireless medium via an antenna, which further receives data and transmits it to processor 701.

[0062] Processor 701 is responsible for managing the bus and general processing, and can also provide various functions, including timing, peripheral interfaces, voltage regulation, power management, and other control functions. Memory 702 can be used to store data used by processor 701 during operation.

[0063] Example 5 This invention also proposes a storage medium storing a path planning method for merging turning costs. When the path planning program for merging turning costs is executed, it implements the steps of the path planning method for merging turning costs as described above. Since this storage medium employs all the technical solutions of all the above embodiments, it possesses at least all the beneficial effects brought about by the technical solutions of the above embodiments, which will not be elaborated upon here.

[0064] The above descriptions are merely embodiments of the present invention. Commonly known structures and characteristics are not described in detail here. Those skilled in the art are aware of all common technical knowledge in the field prior to the application date or priority date, are aware of all existing technologies in that field, and have the ability to apply conventional experimental methods prior to that date. Those skilled in the art can, based on the guidance provided in this application, improve and implement this solution in combination with their own capabilities. Some typical known structures or methods should not be obstacles for those skilled in the art to implement this application. It should be noted that those skilled in the art can make several modifications and improvements without departing from the structure of the present invention. These should also be considered within the scope of protection of the present invention, and will not affect the effectiveness of the implementation of the present invention or the practicality of the patent. The scope of protection claimed in this application should be determined by the content of its claims, and the specific embodiments described in the specification can be used to interpret the content of the claims.

Claims

1. A path planning method that integrates turning costs, characterized in that, Includes the following steps: S1. Construct a path planning graph including nodes and edges, and assign an edge weight value to each edge; S2. In response to the user's route planning request, determine the start and end points of the route; S3. Define the state in the priority queue, which includes the current cumulative cost, the current node, and the predecessor node; S4. Determine if the priority queue is empty. If not, pop the state with the lowest current cumulative cost in the priority queue and jump to step S5. If yes, the path planning ends. S5. Determine whether the current node in the state with the minimum cumulative cost is the destination. If yes, output the path from the starting point to the current node and the corresponding current cumulative cost. If no, jump to step S6. S6. Traverse the next neighbor node of the current node and calculate the turning cost between the current node and the next neighbor node based on the predecessor node. S7. Based on the turning cost, edge weight value, and current cumulative cost, calculate the total cost from the starting point to the next neighbor node; S8. When the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, the total cost from the starting point to the next neighbor node is updated to the priority queue, and then the process jumps to step S4.

2. The path planning method for merging turning costs according to claim 1, characterized in that, The edge is a path segment connecting the node, and the edge weight is the distance or travel time of each edge.

3. The path planning method for merging turning costs according to claim 1, characterized in that, Step S5 includes: S51. Define an identifier for the node. Determine whether the identifier of the current node corresponding to the current state with the minimum cumulative cost is the same as the identifier of the endpoint. If yes, the current node corresponding to the current state with the minimum cumulative cost is the endpoint. Then jump to step S52. If no, jump to step S6. S52. Define a parent node dictionary, backtrack based on the parent node dictionary and output the path from the starting point to the current node and the corresponding current cumulative cost.

4. The path planning method for merging turning costs according to claim 1, characterized in that, Step S6 includes: S61. Determine if the current node has a corresponding predecessor node. If not, the turning cost between the current node and the next neighbor node is 0. If yes, jump to step S62. S62. Define the vector from the predecessor node to the current node as the predecessor vector, and the vector from the current node to the next neighbor node as the current vector. S63. Based on the angle between the predecessor vector and the current vector, calculate the turning cost between the current node and the next neighbor node.

5. The path planning method for merging turning costs according to claim 4, characterized in that, The method for calculating the angle between the predecessor vector and the current vector includes: S631. Calculate the dot product and magnitude based on the predecessor vector and the current vector; S632. Based on the dot product and magnitude, and using the inverse cosine function, calculate the angle between the predecessor vector and the current vector.

6. The path planning method for merging turning costs according to claim 5, characterized in that, The formula for calculating the steering cost is a linear function, and the linear function is: ； In the formula, It is a linear function of the turning cost; The angle between the predecessor vector and the current vector; These are the weighting coefficients.

7. The path planning method for merging turning costs according to claim 5, characterized in that, The formula for calculating the steering cost is a nonlinear function, and the nonlinear function is: ； In the formula, For the nonlinear function of the turning cost; The angle between the predecessor vector and the current vector; These are the weighting coefficients.

8. The path planning method for merging turning costs according to claim 1, characterized in that, The formula for calculating the total cost from the starting point to the next neighbor node is as follows: Total cost = current cumulative cost + edge weight value + turning cost.

9. The path planning method for merging turning costs according to claim 1, characterized in that, In step S8, the total cost from the starting point to the next neighbor node is updated in the priority queue, including: Create a new state based on the total cost from the starting point to the next neighbor node, and update the new state to the priority queue.

10. A path planning system applying the path planning method with fused turning costs as described in any one of claims 1 to 9, characterized in that, include: The graph model building module is used to construct a path planning graph that includes a set of nodes and a set of edges. The request receiving module is used to receive users' route planning requests; The path planning module is used to define and manage the states in the priority queue, calculate the turning cost between the current node and the next neighbor node based on the states in the priority queue, calculate the total cost from the starting point to the next neighbor node based on the turning cost, update the total cost from the starting point to the next neighbor node to the priority queue when the total cost from the starting point to the next neighbor node is less than the historical minimum cost from the starting point to the next neighbor node, and output the path from the starting point to the current node and the corresponding current cumulative cost when the current node corresponding to the state with the minimum current cumulative cost is the destination.

Images