Path planning method, device and equipment and readable storage medium

Abstract

The invention provides a path planning method, device and equipment and a readable storage medium, and the method is applied to a mobile device, and comprises the steps: obtaining a target map, dividing the target map into a plurality of sub-regions, and enabling the plurality of sub-regions to comprise a starting point region and an ending point region; determining an access point in the starting point area, and determining an outgoing point in the ending point area; a hybrid obstacle avoidance algorithm is utilized to determine an access path between the starting point and the access point and determine an outgoing path between the outgoing point and the terminal point, and the hybrid obstacle avoidance algorithm is a path planning algorithm combining a heuristic search algorithm and vehicle dynamics constraints; determining a main road path between the access point and the outgoing point by using a shortest path search algorithm; and determining a target path of the movable device based on the access path, the main road path and the access-out path. The driving requirements of the movable device can be met while the driving path is optimized.

Description

Technical Field

[0001] This invention relates to the field of computer technology, and in particular to a path planning method, apparatus, device, and readable storage medium. Background Technology

[0002] Structured roads refer to road environments with clearly defined and standardized traffic facilities; unstructured roads are areas lacking standardized traffic facilities and clearly defined boundaries. Since intelligent driving vehicles need to simultaneously meet the requirements of obstacle avoidance safety, driving efficiency, and ride comfort when navigating mixed environments including urban road networks, construction zones, structured roads, and unstructured roads, the ability to adaptively determine the driving path of intelligent driving vehicles based on environmental changes is particularly important.

[0003] Current technologies typically employ either global or local planning algorithms for path planning. Global planning algorithms can find the optimal path in structured road networks, but in complex, large-scale environments, they are computationally time-consuming and lack real-time performance, failing to meet the driving requirements of vehicles. Local planning algorithms can handle obstacles in unstructured environments in real time, but lack a global perspective, potentially leading to suboptimal paths or even local minima. The limitations of these two algorithms are particularly pronounced in mixed-mode roads (structured and unstructured), easily resulting in abrupt path changes and curvature discontinuities, causing vehicle control oscillations and impacting driving safety and ride comfort.

[0004] Therefore, existing path planning methods struggle to balance vehicle driving needs with global path optimization. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to address the above-mentioned shortcomings of the prior art by providing a path planning method, apparatus, device and readable storage medium that can optimize the driving path while meeting the driving needs of the mobile device.

[0006] In a first aspect, the present invention provides a path planning method applied to a mobile device, the method comprising:

[0007] Obtain the target map and divide the target map into multiple sub-regions, the multiple sub-regions including a starting region and an ending region, the starting region being the region including the starting position of the movable device, and the ending region being the region including the ending position of the movable device;

[0008] Determine the access point in the starting area and the exit point in the ending area. The access point is the path connection location from the starting point to the main road in the starting area, and the exit point is the path connection location from the main road to the ending point in the ending area.

[0009] Using a hybrid obstacle avoidance algorithm, the access path between the starting point and the access point is determined, and the exit path between the exit point and the end point is determined. The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. The hybrid obstacle avoidance algorithm is used to generate continuous drivable paths that satisfy vehicle dynamics constraints and avoid obstacles in the starting point region and the end point region.

[0010] The shortest path search algorithm is used to determine the main road path between the access point and the outgoing point. The shortest path search algorithm is used to determine the shortest path in the main road network, which is the set of main road segments identified in the road topology of the target map.

[0011] The target path of the mobile device is determined based on the access path, the main road path, and the outgoing path.

[0012] Secondly, the present invention provides a path planning device for use in a mobile device, the device comprising:

[0013] The segmentation module is used to acquire the target map and divide the target map into multiple sub-regions. The multiple sub-regions include a starting region and an ending region. The starting region is the region that includes the starting position of the movable device, and the ending region is the region that includes the ending position of the movable device.

[0014] The first determining module, connected to the dividing module, is used to determine the access point in the starting area and the exit point in the ending area. The access point is the path connection position from the starting point to the main road in the starting area, and the exit point is the path connection position from the main road to the ending point in the ending area.

[0015] The second determining module, connected to the first determining module, is used to determine the access path between the starting point and the access point, and the exit path between the exit point and the end point using a hybrid obstacle avoidance algorithm. The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. The hybrid obstacle avoidance algorithm is used to generate continuous drivable paths that satisfy vehicle dynamics constraints and avoid obstacles in the starting point region and the end point region.

[0016] The third determining module, connected to the second determining module, is used to determine the main road path between the access point and the outgoing point using a shortest path search algorithm. The shortest path search algorithm is used to determine the shortest path in the main road network, where the main road network is a set of main road segments identified in the road topology of the target map.

[0017] The fourth determining module, connected to the third determining module, is used to determine the target path of the mobile device based on the access path, the main road path, and the outgoing path.

[0018] Thirdly, the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the path planning method described in the first aspect above.

[0019] Fourthly, the present invention provides an electronic device including a processor, a memory, and a program or instructions stored in the memory and executable on the processor, wherein the program or instructions, when executed by the processor, implement the path planning method as described in the first aspect.

[0020] This invention provides a path planning method, apparatus, device, and readable storage medium. The method employs different planning algorithms for different regions. A hybrid obstacle avoidance algorithm is used in the starting and ending areas, while shortest path search is used for the main road network. The hybrid obstacle avoidance algorithm ensures that both the access and exit paths effectively avoid obstacles while satisfying vehicle dynamics constraints, achieving path feasibility and safety. The shortest path search algorithm for main road paths simplifies path selection on main roads, improving overall path efficiency. The target path is obtained by combining these three path segments, thereby optimizing the driving path while meeting the driving requirements of mobile devices. Attached Figure Description

[0021] Figure 1 This is a flowchart of a path planning method according to Embodiment 1 of the present invention;

[0022] Figure 2 The following is a reference diagram showing the overall algorithm framework of another path planning method according to Embodiment 1 of the present invention;

[0023] Figure 3 This is a flowchart of the nearest edge finding algorithm in Embodiment 1 of the present invention;

[0024] Figure 4 The flowchart of the parallel Dijkstra algorithm in Embodiment 1 of the present invention is shown below;

[0025] Figure 5 This is a schematic diagram of the left and right turns from the starting point to the end point in Embodiment 1 of the present invention;

[0026] Figure 6 This is a schematic diagram of a U-shaped bend from the starting point to the end point in Embodiment 1 of the present invention;

[0027] Figure 7 This is a schematic diagram of the structure of a path planning device according to Embodiment 2 of the present invention;

[0028] Figure 8 This is a schematic diagram of a device structure according to Embodiment 3 of the present invention. Detailed Implementation

[0029] To enable those skilled in the art to better understand the technical solution of the present invention, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0030] It is understood that the specific embodiments and accompanying drawings described herein are merely for explaining the invention and are not intended to limit the invention.

[0031] It is understood that, without conflict, the various embodiments and features in the embodiments of the present invention can be combined with each other.

[0032] It is understood that, for ease of description, only the parts related to the present invention are shown in the accompanying drawings, while the parts unrelated to the present invention are not shown in the drawings.

[0033] It is understood that each unit or module involved in the embodiments of the present invention may correspond to only one entity structure, or may be composed of multiple entity structures, or multiple units or modules may be integrated into one entity structure.

[0034] It is understood that the terms "first," "second," etc., in the embodiments of the present invention are used to distinguish different objects or to distinguish different treatments of the same object, rather than to describe a specific order of objects.

[0035] It is understood that, without conflict, the functions and steps marked in the flowcharts and block diagrams of this invention may occur in a different order than that marked in the accompanying drawings.

[0036] It is understood that the flowcharts and block diagrams of this invention illustrate the possible architecture, functions, and operations of systems, apparatuses, devices, and methods according to various embodiments of this invention. Each block in the flowchart or block diagram may represent a unit, module, program segment, or code, containing executable instructions for implementing the specified function. Furthermore, each block or combination of blocks in the block diagram and flowchart can be implemented using a hardware-based system to achieve the specified function, or using a combination of hardware and computer instructions.

[0037] It is understood that the units and modules involved in the embodiments of the present invention can be implemented by software or by hardware. For example, the units and modules can be located in a processor.

[0038] It should be noted that the scenario diagrams described in the embodiments of this application are for the purpose of more clearly illustrating the technical solutions of the embodiments of this application, and do not constitute a limitation on the technical solutions provided in the embodiments of this application. As those skilled in the art will know, with the evolution of network architecture and the emergence of new business scenarios, the technical solutions provided in the embodiments of this application are also applicable to similar technical problems.

[0039] To facilitate understanding, the concepts involved will be introduced first.

[0040] Dijkstra's algorithm is a shortest path search algorithm used to find the shortest path from the starting point to all other nodes in a weighted directed or undirected graph. The core of the algorithm is a "greedy strategy": starting from the starting point, it gradually expands to all adjacent nodes, selecting the node with the currently known shortest path for each expansion.

[0041] A* algorithm: A heuristic search algorithm that combines the "actual cost" (distance from the starting point to the current node) and the "heuristic function" (estimate the remaining distance from the current node to the target) of Dijkstra's algorithm.

[0042] With the rapid development of intelligent traffic management and autonomous driving technology, the computational efficiency and path generation quality of algorithms that perform global path planning from origin to destination have become critical issues.

[0043] Traditional path planning methods (such as the global planning algorithm Dijkstra's algorithm and the local obstacle avoidance algorithm A*) can theoretically guarantee optimality, but they still have significant shortcomings in terms of computational efficiency in large-scale road networks and adaptability to scenarios with high path redundancy. Furthermore, they only consider the existence of roads and ignore their directionality. Specific shortcomings are as follows:

[0044] 1. Traditional one-way Dijkstra's algorithm.

[0045] The classic Dijkstra algorithm is a commonly used global planning algorithm. It uses a breadth-first strategy to expand nodes layer by layer. Although it can guarantee the global shortest path, its unidirectional search mode causes the computational complexity to increase quadratically with the number of nodes. Especially in complex road networks, the memory consumption and time cost are too high, and the planning efficiency may be reduced due to blindly searching invalid areas.

[0046] From the perspective of spatial constraints, Dijkstra's algorithm fails to effectively incorporate geometric guidance mechanisms when handling path planning. In real-world geographic spaces or road networks, various spatial constraints exist, such as road orientation and obstacle distribution. Properly utilizing these spatial constraints can significantly improve the efficiency of path planning. However, due to the lack of geometric guidance, Dijkstra's algorithm cannot reasonably limit the search range based on spatial characteristics during the search process.

[0047] Dijkstra's algorithm, a classic graph-based pathfinding algorithm, determines the optimal path in structured road scenarios by calculating the shortest distance from each node to the source node. However, in environments with a mix of structured and unstructured roads, the computational complexity of Dijkstra's algorithm increases dramatically because the unstructured road portion cannot construct a regular and stable graph structure like the structured road portion. Furthermore, its fixed search method is ill-suited to handling the various dynamic changes within unstructured roads.

[0048] Therefore, Dijkstra's algorithm is computationally time-consuming and has poor real-time performance in complex and large-scale environments, and it cannot meet the driving needs of vehicles.

[0049] 2. A* Algorithm.

[0050] The A* algorithm is a common local obstacle avoidance algorithm. In structured road scenarios, the A* algorithm can efficiently search for the optimal path from the starting point to the ending point by calculating the sum of the estimated cost and the actual cost of each node in an ideal structured map environment. However, when unstructured situations arise in structured roads, such as temporary road closures or changes in traffic rules due to construction on some road sections, the fixed heuristic function and search strategy on which the A* algorithm relies become difficult to adapt quickly to these dynamic changes. Over-reliance on the heuristic function may cause it to miss the global optimum, thus losing its original path search flexibility.

[0051] Therefore, although the A* algorithm can deal with obstacles in unstructured environments in real time, it lacks a global perspective and may lead to suboptimal paths or even local minima traps.

[0052] In summary, when faced with complex real-world environments where structured and unstructured roads coexist, many existing path planning algorithms have failed to reasonably and effectively consider how to perform accurate, efficient, and flexible path planning in such mixed road conditions.

[0053] To address the aforementioned technical issues, this application provides a path planning method to improve the efficiency and adaptability of path planning.

[0054] Example 1:

[0055] This embodiment provides a path planning method, such as Figure 1 As shown, it is applied to mobile devices. In this application, a mobile device refers to a device that has autonomous or controlled movement capability and can move autonomously or in a controlled manner according to a preset path or a real-time planned path. It is suitable for different application scenarios and working environments, such as automobiles, inspection robots, etc.

[0056] The method specifically includes the following steps S101 to S105:

[0057] S101: Obtain the target map and divide the target map into multiple sub-regions, including the starting area and the ending area.

[0058] The starting area is the area including the starting position of the movable device, and the ending area is the area including the ending position of the movable device.

[0059] Specifically, the current global map is read online or offline, and the map is divided into sub-regions based on factors such as traffic flow, road facilities, and traffic environment, resulting in sub-regions with different geometric shapes.

[0060] By structuring the overall complex map space, it becomes easier to carry out subsequent segmented planning and processing of key areas.

[0061] S102: Determine the access point in the starting area and the exit point in the ending area.

[0062] Specifically, within the starting area, a point is selected as the access point connecting to the main road; within the ending area, a point is selected as the exit point connecting to the main road. These two points will connect the local paths within the sub-area with the main road paths, acting as a bridge.

[0063] S103: Using a hybrid obstacle avoidance algorithm, determine the access path from the starting point to the access point, and determine the outgoing path from the outgoing point to the end point.

[0064] The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. It is used to generate continuous drivable paths that satisfy vehicle dynamics constraints and avoid obstacles in the starting and ending regions.

[0065] Specifically, a hybrid obstacle avoidance algorithm is used for path planning, from the starting point to the access point and from the exit point to the end point. This hybrid obstacle avoidance algorithm combines heuristic search methods (such as A*) with vehicle dynamics constraints (such as maximum steering angle, minimum turning radius, etc.) to ensure that the generated path not only avoids obstacles but can also be safely and smoothly executed by actual vehicles and other mobile devices.

[0066] S104: Use the shortest path search algorithm to determine the main road path between the main road access point and the main road exit point.

[0067] The shortest path search algorithm is used to determine the shortest path in the main road network. Shortest path search can be performed based on the main road network topology and edge weights (length / speed limit / traffic constraints). The main road network is the set of main road segments identified from the road topology of the target map (identified based on road class / speed limit / connectivity thresholds).

[0068] Specifically, shortest path search algorithms (such as the common Dijkstra's algorithm and A*) are used to plan the optimal path between access points and outgoing points in the main road network. Depending on the actual situation, a suitable path planning algorithm can be selected considering search efficiency, the mileage of the main road segments, and traffic characteristics. Since main roads typically have a well-defined structure, they can be planned efficiently.

[0069] S105: Determine the target path of the mobile device based on the access path, main road path, and outgoing path.

[0070] Specifically, the three paths mentioned above—the access path, the main road path, and the outgoing path—are connected and integrated to obtain the final complete target path from the starting point to the destination. The resulting target path takes into account both local regional constraints and main road traffic efficiency, making it the optimal solution for mobile devices.

[0071] In this embodiment, the complex overall path is decomposed into several functionally defined segments for separate planning. This allows for the adoption of more suitable algorithms based on the environment and needs of different areas (e.g., a hybrid obstacle avoidance algorithm is used for the starting and ending areas, while the shortest path search is used for the main road). This segmented planning method improves the efficiency and adaptability of path searching. The hybrid obstacle avoidance algorithm ensures that both the access and exit paths effectively avoid obstacles while meeting vehicle dynamic constraints, thus achieving path feasibility and safety. The use of the shortest path search algorithm for the main road simplifies path selection on the main road, improving the overall path's traffic efficiency. Finally, through the organic combination of these three path segments, a target path that simultaneously considers safety, feasibility, and efficiency is generated, better meeting the path planning needs of mobile devices in complex environments.

[0072] Optionally, determining the connection point with the main road in the starting area in S102 above specifically includes:

[0073] Acquire information on main roads within the starting area, the first heading angle of the mobile device at the starting point, and the starting point position. The main road information includes all main road segments within the starting area. Calculate the Euclidean distance between the starting point and the nearest point of each main road segment. Calculate the heading difference between the first heading angle and the heading angle of each main road segment. Based on the Euclidean distance and heading difference corresponding to each main road segment, determine the target main road segment within the starting area. Determine the nearest point from the starting point to the target main road segment as the access point.

[0074] Specifically, first, relevant information about the main roads within the starting area is obtained, including data on all main road segments; and the specific location of the movable device at the starting point and its first heading angle (i.e., the orientation of the movable device at the starting point). The heading angle can be determined based on the road tangent direction or based on local geometric fitting. Therefore, the tangent direction of the target main road segment at the access point can be determined as the second heading angle of the access point.

[0075] Then, for each main road segment obtained above, calculate the Euclidean distance from the starting point to the nearest point on that segment. That is, find the minimum distance from the starting point to each main road segment.

[0076] Then, for each main road segment, the heading difference between the first heading angle of the mobile device's starting point and the heading angle of that segment is calculated. The heading angle refers to the angle of travel direction for the corresponding segment, describing the geometric orientation of the main road segment.

[0077] Combining the aforementioned Euclidean distance and heading difference indicators, each main road segment is evaluated. Based on the distance and direction consistency, the optimal target main road segment is determined as the access object. The selection criteria can be based on a weighted strategy of minimum distance and minimum heading difference, etc.

[0078] Finally, the nearest point on the main road segment from the starting point to the target is set as the access point, serving as the connection point for subsequent path planning.

[0079] In this embodiment, by considering the physical distance and heading compatibility between the starting point and the main road segment, and taking into account both the actual accessibility of the path and the rationality of the driving direction, a more suitable access point for the current state of the mobile device can be selected. This effectively reduces unnecessary turning and movement distances during access, simplifies the difficulty of local path planning, and improves the naturalness and feasibility of the path.

[0080] Optionally, the process in S103 above, which uses a hybrid obstacle avoidance algorithm to determine the access path from the starting point to the access point, specifically includes:

[0081] Based on obstacle information in the starting region, the positions of the starting point and the access point, the first heading angle of the starting point and the second heading angle of the access point, a state space for the starting region is created. The state space is used to represent all possible motion states of the mobile device in the starting region. Based on the kinematic constraints of the mobile device and the obstacle distribution, the travel cost of each node in the state space is calculated. Based on the travel cost of each node, a search strategy based on a heuristic cost function is used to perform path search in the state space to generate an access path between the starting point and the access point that simultaneously satisfies collision-free and kinematic constraints.

[0082] The second heading angle can be determined by the tangent direction of the main road section where the access point is located.

[0083] Specifically, firstly, based on obstacle information within the starting area, the positions of the starting point and the access point, and the first heading angle of the starting point and the second heading angle of the access point, a state space for the area is established. This state space is used to express all possible states of the "mobile device" within the starting area. The states are typically combined with position and heading variables, for example, expressed as (x, y, θ) triples.

[0084] Then, based on the kinematic constraints of the movable device (such as maximum steering angle, minimum turning radius, etc.) and the current obstacle distribution, the travel cost of each node in the state space is calculated. The travel cost usually includes multiple factors, such as distance, steering cost, obstacle avoidance cost, etc., which together constitute the comprehensive evaluation index of the node and are used as the basis for decision-making in subsequent path search.

[0085] Finally, based on the calculated travel cost of each node, a heuristic cost function search strategy is employed to perform path search in the state space. The heuristic cost typically combines the actual cost from the starting point to the current node and the estimated cost from the current node to the target, guiding the search in the most promising direction. By continuously expanding and evaluating nodes, a path from the starting point to the access point is ultimately generated, satisfying both collision-free requirements and the kinematic constraints of the movable device.

[0086] In this embodiment, a state space containing motion state and heading information is constructed, and legal path expansion is defined by combining kinematic constraints and obstacle data. This ensures that the generated access path is both collision-free and meets the dynamics and motion capability requirements of the mobile device. A heuristic cost function search strategy efficiently guides the path search process, improving solution speed and path quality, and effectively ensuring the safety and feasibility of the path planning results.

[0087] In some embodiments, the same method can be used to determine the nearest exit point to the main road in the sub-region where the endpoint is located, which will not be elaborated here.

[0088] Optionally, the calculation of the travel cost for each node in the state space based on the kinematic constraints of the mobile device and the obstacle distribution may specifically include:

[0089] Based on the current node's position coordinates and heading angle, combined with preset steering angle and driving direction parameters, the kinematic equations of the movable device are applied to calculate the position coordinates and heading angle of the potential next node. For each calculated potential next node, obstacle collision detection is performed to filter out collision-free feasible nodes. For each feasible node, the incremental driving cost from the current node to the feasible node and the heuristic cost are calculated. The cumulative cost of the current node is added to the incremental driving cost of the feasible node to obtain the cumulative cost of the feasible node. The cumulative cost of the feasible node is determined as the driving cost of the feasible node.

[0090] The incremental travel cost includes at least the travel distance cost, travel direction switching cost, steering angle cost, and steering angle change cost. The heuristic cost is the estimated cost from the current feasible node to the target node; that is, the heuristic cost is the minimum estimated cost from a feasible node (child node) to the target node (path endpoint). The heuristic cost guides the search towards the target direction, improving search efficiency while maintaining acceptability to ensure optimality.

[0091] In this embodiment, the travel distance cost is proportional to the travel step length, and an additional penalty coefficient can be applied when traveling backward; the travel direction switching cost is generated when switching between forward and backward; the steering angle cost is proportional to the absolute value of the steering angle; and the steering angle change cost is proportional to the change in the current steering angle relative to the steering angle of the previous state.

[0092] Here, the steering angle is the deflection angle of the front wheels relative to the vehicle's longitudinal centerline, determining the curvature of the vehicle's turn. The driving direction typically refers to either forward or backward movement. Preset steering angle and driving direction parameters are a set of discretized control input values ​​used when calculating possible next-node states. These parameters are predefined to simulate possible vehicle motion patterns. The design of steering angle parameters typically considers: the vehicle's physical steering limits (e.g., usually around ±35°); the balance between computational efficiency and path smoothness (fineer angle division results in a smoother path, but also increases computational complexity); and the vehicle's kinematic characteristics (such as wheelbase, minimum turning radius, etc.).

[0093] Specifically, based on the position coordinates and heading angle of the current node, and combined with pre-set different steering angles and driving direction parameters (forward or backward), the kinematic equations corresponding to the movable device (such as the motion equations of nonholonomic constraints) are applied to calculate a series of possible next node states (i.e. potential next nodes), including the position coordinates and heading angle of the next node.

[0094] Then, for each potential next node, obstacle collision detection is performed to determine whether the path from the current node to that node will collide with an obstacle; all next nodes without collision risk are filtered and retained, and the next nodes without collision risk are determined as feasible nodes to ensure path safety.

[0095] For each feasible node after filtering, the incremental travel cost from the current node to that feasible node is calculated. This incremental travel cost can specifically include four aspects: travel distance cost, reversal cost, steering angle cost, and steering change cost. Then, the heuristic cost of the feasible node is calculated. The heuristic cost can be obtained by querying a pre-built heuristic map (hmap), which stores the minimum cost estimate from each location in the map to the destination; therefore, for any feasible node n, h(n) represents the estimated cost "from n to the destination".

[0096] Finally, the existing cumulative cost of the current node is added to the calculated incremental travel cost of the feasible node to obtain the cumulative cost of the feasible node. Adding this cumulative cost to the heuristic cost yields the travel cost of this feasible node, which is used for evaluation in subsequent node expansion and path search.

[0097] In this embodiment, during the path search process, the actual travel distance of the mobile device from the starting state to the current state along the planned path is calculated, and the distances of each segment are added together as the travel distance cost.

[0098] During state transitions or path expansion, it is determined whether the movable device has changed its direction of movement (e.g., from forward to backward or vice versa). Whenever a reversal occurs, a reversal cost is accumulated to penalize unnecessary reversals. Each state in the path corresponds to a turning angle. The absolute value of the turning angle or some function of it is statistically accumulated across all states on the path, with larger turning angles incurring higher costs to encourage smaller, gentler turns. The change in turning angle between adjacent states in the path is calculated and accumulated as the turning change cost. The larger the change, the higher the cost, penalizing overly abrupt or frequent turning changes. In the search algorithm, whenever a state expands to a new node, the increase in the above four costs is calculated and added to the actual cumulative cost of the current path, serving as an important component of the heuristic cost function and participating in the determination of subsequent node priorities and path optimization.

[0099] In this embodiment, by refining the actual cumulative cost into four aspects—driving distance, direction change, steering angle, and steering change—the overall advantages and disadvantages of the path scheme are characterized from multiple levels. This effectively suppresses behaviors that are detrimental to the motion stability and safety of the mobile device, such as frequent direction changes, sharp turns, and violent operations. As a result, the final generated path is smoother, more natural, and more suitable for actual execution, thereby significantly improving the feasibility and driving quality of the overall path.

[0100] In some embodiments, the same method can be used to determine the exit path between the exit point and the destination using a hybrid obstacle avoidance algorithm, which will not be elaborated here.

[0101] Optionally, in S104 above, the shortest path search algorithm is used to determine the main road path between the access point and the outgoing point, specifically including: in the main road network, the shortest path between the access point and the outgoing point is calculated based on the parallel shortest path search algorithm.

[0102] Among these, the parallel shortest path search algorithm can be a parallelized implementation of the shortest path search algorithm: while maintaining the optimality of the shortest path, the standard Dijkstra algorithm is modified to be "regionally parallelized." By dividing the global trunk road network into sub-regions, the extended Dijkstra search in each sub-region can be executed simultaneously on a multi-core CPU, and cross-region results are merged at the region boundaries using gateway nodes as bridging points. Alternatively, it can be a shortest path algorithm based on bidirectional search.

[0103] Specifically, firstly, a data structure for the main road network has been established in the path planning system. This road network consists of nodes representing main road intersections and edges representing main road segments, comprehensively describing the connectivity and traffic attributes of the main roads. The access points and exit points on the main road network are known; these two points correspond to the positions where the mobile device enters and leaves the main road network, and are the start and end nodes for path calculation. Using a parallel shortest path search algorithm (this application does not limit the specific implementation), the shortest path between the two points is found in the main road network, starting from the main road access point and ending at the main road exit point.

[0104] Here, parallel processing decomposes the computational task and distributes it to multiple computing units for concurrent execution, thereby improving search efficiency and response speed. After parallel computation is completed, the optimal path from the main access point to the main exit point is output.

[0105] The shortest path usually refers to the path with the lowest cost (such as the shortest distance or the least time) and is used to guide the actual travel of mobile devices on main roads.

[0106] In this embodiment, by employing a parallel shortest path search algorithm for path calculation within the main road network, the planning time for the optimal path on the main roads can be significantly shortened, making it particularly suitable for large-scale road networks. This improves the computational efficiency and real-time performance of path planning, enabling mobile devices to obtain the optimal driving plan for main road segments more quickly, thereby enhancing the overall performance and responsiveness of the path planning system.

[0107] In a specific embodiment, the above path planning algorithm can be implemented as follows: First, find the sub-regions of the starting point and the ending point. In each sub-region, traverse all lane lines for the starting point and the ending point to find the nearest entry path. Then, weighted consideration is given to the Euclidean distance and heading angle difference. Traverse the points of this path and find the point with the smallest weighted value as the target of local path planning. Then, generate a local road segment from the starting point to this point. At the same time, based on the characteristics of network segmentation, use the parallel Dijkstra algorithm to find the optimal planning from the nearest edge of the starting point to the nearest edge of the ending point. Then, use Bezier curves or straight line interpolation for the discontinuous lane lines at the endpoints of adjacent arterial roads to adaptively generate road segments that meet the vehicle driving curvature constraints. Finally, according to the spatial order from the starting point to the ending point, concatenate the local path points with the path points of the arterial roads to obtain the required path.

[0108] like Figure 2 The algorithm framework shown in the reference diagram illustrates the main steps of the path planning method as follows:

[0109] Step 1: Obtain all global road information of the known map, excluding static obstacles in the global road segment.

[0110] Step 2: From the current starting point, use the path planning algorithm to reach the nearest global path segment (access path), and denote the intersection point as point A (access point). The path planning algorithm is a hybrid A*.

[0111] Step 3: Use a path planning algorithm to find the nearest global path segment (outgoing path) from the target point, and denote the intersection point as point B (outgoing point);

[0112] Step 4: From the route point where point A is located to the route segment where point B is located, first divide the road network, and then use the parallel Djkstra algorithm to find the optimal driving route (main road path).

[0113] Step 5: Consider special road sections such as left turns, right turns, and U-turns, and perform interpolation (transition path optimization) at the endpoints of discontinuous road sections to conform to the curvature of vehicle travel.

[0114] Step 6: Output the trajectory (i.e., obtain the target path).

[0115] In another embodiment, a path planning method for complex bidirectional road networks based on parallel Dijkstra's algorithm is provided. This method combines the hybrid A* obstacle avoidance algorithm with the parallel Dijkstra algorithm. By constructing local planning from the starting point and the destination to the main road and bidirectional main road planning, the application scenarios of the path planning algorithm are significantly enriched.

[0116] Specifically, the steps include the following:

[0117] Step 1: Map Segmentation and Sub-region Division (Obtain the target map and divide it into multiple sub-regions).

[0118] The current global map is read online or offline. Based on factors such as traffic flow, road facilities, and traffic environment, the map is divided into sub-regions with different geometric shapes. The division results are shown below:

[0119] Map j :[region1,region2,...,region i ,...,region N ], i = 1, ..., N; where, Map j For a set of sub-regions, region i Let N be the i-th sub-region, and N be the number of sub-regions.

[0120] In step 1, by comprehensively analyzing multi-dimensional features such as traffic flow, road facilities, and traffic environment, an adaptive geometric partitioning strategy is adopted to intelligently decompose the complex global map into several sub-regions, thereby reducing the computational complexity of path planning, optimizing algorithm performance, and providing a more refined and efficient computational foundation for subsequent path search. Under the premise of ensuring regional connectivity and computational efficiency, the optimal segmentation of the map is achieved.

[0121] Step 2: Locate the starting point information.

[0122] Obtain the first heading angle and starting position of the mobile device at the starting point.

[0123] Assuming the aforementioned mobile device is a vehicle, the vehicle's location information S: [x1, y1, θ1] at the starting point can be read based on the vehicle's GPS positioning. Here, x1 and y1 are the vehicle's position coordinates, and θ1 is the vehicle's true heading angle (i.e., the first heading angle). This angle can be the angle between the vehicle's forward direction and the eastward direction. This angle increases counterclockwise and ranges from [0, 360°].

[0124] Step 3: Find the nearest main road and confluence point to the starting point (i.e., determine the access point).

[0125] Find the subregion where the starting point is located. kIf there are m main roads {road} in this area i}, i = 1, ..., m; Find the nearest edge (road) on the global trunk network from the current starting point. j For detailed steps, please refer to [link / reference]. Figure 3 Traverse the edge and find the point with the smallest deviation from the starting point in the heading angle as the access point A1:[x2,y2,θ2]. A1 is determined by formula (1):

[0126] Among them, (x i ,y i ) represents the coordinates of a point on the candidate line, and w1 represents the distance from the starting point to the road. j The Euclidean distance weights for each point, where w2 is the distance from the starting point to the road. j The weight of the heading angle deviation at each point, θ i The direction angle of the line segment is taken into account when merging into the road. j For the comfort of the edges, in this embodiment, w2 = 2 and w1 = 1.

[0127] like Figure 3 As shown, this demonstrates the precise location and selection of the nearest lane line within a road network. j The specific steps can be as follows:

[0128] (1) Given a point P = [x, y] and a set of lane lines Set_line = [a, b, c, d].

[0129] Assuming P is the starting point, obtain the position coordinates [x, y] of point P, obtain all main road segments within the main road area, and obtain the lane line set Set_line.

[0130] (2) Taking the distance from point P to side a as an example, let the perpendicular segments of the two endpoints of side a be [a1, a2].

[0131] Using the two endpoints of line segment a as boundaries, draw a straight line perpendicular to line segment a, denoted as [a1, a2]. This line is used to determine whether the "orthogonal projection" from point P to a falls within the projection range of the line segment.

[0132] (3) Determine whether point P is within the region formed by a1 and a2.

[0133] That is, determine whether the orthogonal projection of P lies between the endpoints (within the area enclosed by [a1, a2]). If yes, proceed to step (5); otherwise, proceed to step (4).

[0134] (4) d is the minimum value from point P to the two endpoints of line segment a.

[0135] If point P is not within the region formed by a1 and a2, i.e., the projection is not within the segment, then the nearest point must be one of the endpoints. Therefore, we take the smaller distance from the starting point P to the two endpoints and determine it as the distance d from the starting point to edge a.

[0136] (5) The distance d from point P to a is the length of the perpendicular segment of line segment a.

[0137] If point P is within the region formed by a1 and a2, the shortest distance is the perpendicular distance. Therefore, the length of the perpendicular segment from the starting point P to line segment a is determined as the distance d from the starting point P to side a.

[0138] (6) Traverse Set_line, the edge with the smallest d is the target edge.

[0139] Repeat the above distance calculation for all line segments in Set_line, and select the line segment with the smallest d to obtain the geometrically closest candidate main road line segment.

[0140] In this embodiment, a "heading difference Δθ" can also be added for joint determination. Calculate the direction angle θa of each line segment; calculate the heading difference Δθ; use formula (1) to determine the target line segment according to the joint rule of "d+Δθ", and the access point obtained thereby can simultaneously meet the requirements of distance and driving comfort.

[0141] Step 4: Read the obstacle information of the sub-region where the starting point is located.

[0142] Obs reads all obstacle information in the sub-region where the starting point is located. j The general (partition, obstacle) information is represented as follows:

[0143]

[0144] In the above formula,

[0145]

[0146] Where j represents the j-th sub-region, and i represents the i-th obstacle. These represent the coordinates of the top-left, bottom-left, top-right, and bottom-right corners of the i-th obstacle in the j-th sub-region, respectively.

[0147] Step 5: Use the hybrid A* algorithm to plan the local obstacle avoidance path from the starting point to the confluence point.

[0148] Within the sub-region of the starting point, a hybrid A* algorithm is used as the local obstacle avoidance algorithm to plan a local obstacle avoidance path Path1 from the starting point S to the ending point A1. Path1 is the access path mentioned above.

[0149] Within the starting sub-region, the Hybrid A* algorithm is applied to generate path Path1 from the starting point S to the access point A1. Hybrid A* combines the heuristic search of A* with vehicle dynamics constraints (such as turning radius and nonholonomic constraints) to avoid obstacles.

[0150] In one example, the hybrid A* algorithm (i.e., the hybrid obstacle avoidance algorithm mentioned above) is implemented using the following steps:

[0151] (1) The A* algorithm calculates the node cost (i.e., calculates the driving cost of each node).

[0152] The cost of a node consists of two parts: the actual cost g(n) from the starting node to the current node (i.e., the cumulative cost mentioned above) and the heuristic cost h(n) from the current node to the target node. It is calculated using the following formula (2):

[0153] f(n) = g(n) + H_COST × h(n) (2)

[0154] Where f(n) is the travel cost of a node, g(n) is the cumulative cost, h(n) is the heuristic cost, and H_COST is the penalty coefficient of the heuristic cost.

[0155] (2) The calculation process of g(n) is as follows:

[0156] The iterative formula for the desired path point is shown in formula (3) below. Formula (3) determines the state of the next node after one iteration, which is the update process from the state of the current node to the state of the next node:

[0157]

[0158] Where, x i and y i yaw represents the position coordinates of the current node. i Let d be the heading angle of the movable device at the current node, d be the travel distance of the movable device, and x be the heading angle of the movable device. i+1 and y i+1 Let pi2pi(·) be the position coordinates of the next node, yaw be the angle normalization function. i+1 Let WB be the heading angle of the next node, WB be the wheelbase of the vehicle, and u be the steering angle of the movable device.

[0159] If x i+1 y i+1 If a point is within the obstacle range, it is abandoned, thus selecting the next feasible node.

[0160] When calculating the cost of the next feasible node, several factors need to be considered, including driving direction and steering angle. The cumulative driving cost of the next node is calculated using the following formulas (4) and (5):

[0161] g(n next )=g(n curr )+Δg (4)

[0162] Δg=Δg distance +Δg gear +Δg steer_yaw +Δg steer_change (5)

[0163] Wherein g(n) next ) represents the cumulative cost of the next node, g(n) curr ) represents the cumulative cost of the current node, and Δg represents the incremental cost of the next node.

[0164] Δg distance As a cost for travel distance, if we move forward, then Δg distance =step, where step is the step size; if moving backward, then Δg distance =step*B_COST, where B_COST is the backoff cost coefficient.

[0165] Δg gear The cost of switching driving directions is Δgear = G_COST, where G_COST is the penalty coefficient for switching directions.

[0166] Δg steer_yaw As a cost for the steering angle, Δg steer_yaw =STEER_COST×|u|, where STEER_COST is the steering angle penalty coefficient.

[0167] Δg steer_change For the cost of angle change,

[0168] Δg steer_change =STEER_CHANGE_COST × |n curr .steer-u∣, STEER_CHANGE_COST is the steering change penalty coefficient, n curr .steer is the steering angle of the current node, and u is the steering angle of the movable device.

[0169] (3) Calculation process of h(n).

[0170] The heuristic distance can be quickly obtained from the heuristic map, where each point represents the Manhattan distance cost from the current point to the destination. If the current point is an obstacle, the Manhattan cost is infinite. This method trades space for time, reducing real-time computation of the current node and improving computational efficiency. Specifically, before the main search, a heuristic map hmap is generated offline, enabling the system to quickly obtain the estimated minimum cost h(n) from n to the destination by looking up a table when expanding any node n, thereby improving search efficiency.

[0171] In one example, the heuristic map construction process is as follows:

[0172] Step 1: Reverse A* search initialization.

[0173] 1) Obtain the continuous coordinates of the endpoint (xg, yg), map parameters (resolution res, origin offset x0, y0, grid size), and the size information of the obstacle map obsmap.

[0174] 2) Using the target point (i) g ,j g Starting with q, initialize the open set open_set and priority queue q_priority; the initial cost of the target node is g(n). goal If ) = 0, add it to the priority queue and sort it by cost.

[0175] Specifically, the endpoint is treated as the source of the single-source shortest path, and "backward cost propagation" is performed. The minimum cost from each grid to the endpoint is calculated and written into a table. The starting point and data structure of the backward propagation are determined so that the front boundary of the cost propagation from the endpoint can be correctly initiated by the algorithm and continuously executed. Step 2: Backward cost propagation process.

[0176] Calculate the minimum cost from each reachable grid to the endpoint, and make that cost a lower bound for subsequent heuristic values.

[0177] 1) Remove the node n with the minimum cost from the priority queue. curr .

[0178] 2) Expand all its adjacent nodes n next Adjacent nodes are determined by the following formula (6):

[0179] n next =(i curr +Δi,j curr +Δj,g(n curr )+cost(Δi,Δj)) (6)

[0180] Among them, i curr and j curr For the current node n curr The grid coordinates (rows and columns) are given, (Δi, Δj) is the preset movement direction (e.g., 8-neighborhood), and cost is the movement cost function.

[0181] 3) For each n next Perform a validity check.

[0182] Specifically, this includes: (1) whether the grid index is within the map area; (2) whether it collides with the obstacle map (obsmap); (3) if n nextIf it is effective and more cost-effective, then update the open set and priority queue.

[0183] Step 3: Heuristic Map Generation

[0184] Initialize a two-dimensional array `hmap` with a size equal to the grid map size, initially set to infinity; iterate through all nodes in the closed set `closed_set`, and write the minimum cost from each grid position to the target point into a cheat table: `hmap[i-mini][j-minj] = g(n(i,j))`, where (minj) = g(n(i,j)). i ,min j This is the minimum grid index for the map. The minimum cost results obtained in Step 2 are organized into a "cheat sheet" for quick online querying by the main search engine.

[0185] The hybrid A* algorithm calculates the minimum cost value from the endpoint for each neighboring node and records it. This minimum cost value is then used to generate the desired path (i.e., the access path). The method for determining the outgoing path is the same as the method for determining the access path described above, and will not be repeated here.

[0186] Step 6: Locate the destination information.

[0187] Obtain the first heading angle and the endpoint position of the mobile device at the endpoint. The endpoint positioning information is E1:[x3,y3,θ3].

[0188] Step 7: Locate the nearest main road and merging point to the destination (determine the exit point in the destination area).

[0189] Specifically, find the subregion where the endpoint is located. j Then, following the rules in step 3, find the endpoint A2:[x4,y4,θ4] that is closest to the global backbone network.

[0190] Step 8: Read the obstacle information of the sub-region where the destination is located.

[0191] Obs reads all obstacle information in the sub-region where the destination is located. 2 .

[0192] Step 9: Use the hybrid A* algorithm to plan the local obstacle avoidance path from the confluence point to the destination.

[0193] Within the sub-region where the endpoint is located, a hybrid A* algorithm is used to plan a local obstacle avoidance path Path2 from the starting point A2 to the endpoint E.

[0194] Step 10: Use the parallel Dijkstra algorithm to plan the optimal path A1→A2 of the main road (i.e., use the shortest path search algorithm to determine the main road path between the main road access point and the main road exit point).

[0195] Using Dijkstra's algorithm with parallel computation, find the optimal main road that minimizes the distance cost from A1 to A2:

[0196] Path main :[path1,path2,...,path i ,...,path N ], i = 1, ..., N;

[0197] like Figure 4 As shown, the specific steps of Dijkstra's algorithm for parallel computing are as follows:

[0198] 1. The road network is divided into sub-regions based on traffic flow, facilities and environmental factors.

[0199] The overall road network is divided into region1…region N N sub-regions (e.g., divided by geographic blocks, functional zones, traffic density, or topological bottlenecks).

[0200] 2. In each sub-region, find the points Ai and Bi that have the smallest Euclidean distance from the start point and the end point.

[0201] 3. Determine whether Ai is equal to Bi.

[0202] If Ai = Bi, then the closest point to the start and end points within a certain region is the same point, indicating that this region contributes nothing to the current cross-region connection or has no effective traversal path. Therefore, this sub-region is discarded. i They will not be included in this round of candidates.

[0203] If Ai ≠ Bi, then the sub-region may carry traversal or transit paths from the origin to the destination, thus defining the region. i Add to the "Possible Region Set".

[0204] 4. Obtain the new Map sub-region network.

[0205] Reconstruct submaps Map' = [region] from the filtered regions 1n ,region 2n ,…,region Mn This process eliminates non-contributing regions and retains connected candidate regions and their adjacency relationships. It reduces the computational domain of subsequent parallel Dijkstra's operations, improving efficiency and robustness.

[0206] 5. Within each candidate region, perform Dijkstra's calculation from Ai to Bi.

[0207] In region i Find the shortest path on the internal road network (fine-grained graph) to obtain the "path" within the area.i Its cost weight i The number of the "next adjacent area" leading from this area (determined by the exit-side boundary).

[0208] 6. Use the weight of each region as the weight from the current region. i to adjacent regions j The "cross-region weights" are used to abstract the shortest traversal cost within each region into region-level edge weights, thus constructing a region-level hypergraph.

[0209] 7. Apply Dijkstra's algorithm again to the set of subregions that have been pruned and computed in parallel to obtain the optimal region sequence.

[0210] Running Dijkstra's algorithm on the regional hypergraph yields a minimum total cost sequence (Map) from the starting region to the ending region. result =[region s ,…,region e ].

[0211] 8. Concatenate the components sequentially to obtain the main road path. main .

[0212] Step 11: Determine the locations of A1 (access point) and A2 (outgoing point) on the main road path.

[0213] Find the index of point A1 on the right-hand drive section of path1. Find point A2 on the path N Index of road segment in the right-hand driving direction

[0214] Specifically, the main road path obtained in step 10 is a main road path composed of multiple segments. main Determine the "index subscripts" of A1 and A2 in their respective road segments from the main road path, and denot them as follows: and The index is the position number of a point in the sequence of points within a road segment. Obtaining an accurate index ensures that the path's direction of travel is consistent with the index increment, preventing subsequent slicing, splicing, and mileage calculations from reversing.

[0215] Step 12: Correct the main road route.

[0216] For the Path in step 10 main path1 and path N The corrections are made separately, and the corrections are as follows: The remaining elements of path1 remain unchanged, resulting in the updated Path. main The main road Path is about to be completed.main The trimming is limited to the area between access point A1 and exit point A2, retaining only the main road sections that vehicles will actually travel on.

[0217] Step 13: Generate transition paths between adjacent road segments.

[0218] For adjacent road segments in step 11 i ,path i+1 Generate a suitable transition path based on whether they are parallel or intersecting. Then, each pair of adjacent road segments is processed to obtain the following set of transition road segments: Path aux The expression:

[0219]

[0220] This solves the problem of paths between adjacent main road segments. i with path i+1 To address potential geometric issues such as "discontinuities, non-parallelism, and different lane centers," feasible and smooth connection segments are generated. This enables the vehicle to travel continuously.

[0221] Step 14: Connect the main road path and the transition path.

[0222] For the trajectory Path in steps 12 and 13 main and Path aux Seamlessly connect the components in the correct order to form a continuously traversable global path. The resulting global trunk road path is as follows:

[0223]

[0224] In the above formula, This represents the i-th main path and the subsequent concatenated path.

[0225] Consider starting from path i to path i+1 To accommodate the possibilities of left turns, right turns, U-turns, and straight-ahead maneuvers, and considering the curvature constraints and spatial geometric constraints of turning, Bezier fitting and linear interpolation are used to supplement the trajectory of points, ensuring continuity of position, direction, and optimal curvature, while also satisfying feasibility constraints such as the minimum turning radius of the vehicle. Specifically, linear interpolation can be used to handle smooth transitions with small angles and short distances, while Bezier fitting is used to handle smooth connections requiring steering.

[0226] (1) The formula for Bessel fitting is as follows: Formula (7):

[0227] If P0 = (x0, y0), P1 = (x1, y1), and P2 = (x2, y2), then:

[0228]

[0229] Here, P0 is the starting point of the Bézier curve. In path splicing, this point is usually taken from the segment of the main road. i The last point (path) i P1 is the control point of the Bézier curve; P2 is the endpoint of the Bézier curve. In path splicing, this point is taken from the transition path. The first point; t is a parameter variable. The value of t is between [0,1]. When t=0, the curve is located at the starting point P0, when t=1, the curve is located at the ending point P2; when t is in the middle value, the curve forms a smooth arc between P0 and P2, attracted by P1.

[0230] (2) The formula for linear interpolation is as follows: Formula (8):

[0231] If P0 = (x0, y0), P1 = (x1, y1), the sampling distance is d meters, and the sampling includes point P1, the calculation formula is as follows:

[0232]

[0233] in,

[0234] P1 is the starting point of the line segment, which can be the path. i The end point or transition segment path ti A point on the line. P2 is the endpoint of the line, which can be the path. i+1 The starting point or transition path ti A point; d is the sampling distance (the target interval between two adjacent discrete points), used to control the point density, often matched with vehicle speed / control cycle; L is the Euclidean distance between the two endpoints, used to normalize the direction vector and determine the number of sampling points; k is an integer index, indicating the kth interpolation point.

[0235] like Figure 5 In the example shown, the starting point is assumed to be in area A and the ending point is in area B. The base map shows the main roads and ramp structure of the urban road network. Figure 5 This displays the complete main driving path from the starting point in area A to the ending point in area B. The green trajectory represents the final main path obtained after processing in steps 12 and 13, and then stitching and adding points in step 14. main_aux The path is formed by connecting the "pruned main trunk segment" and "multiple inter-segment transition segments"; at the boundary of area A, the path merges into the first main trunk segment path1; near the boundary of area B, it merges into the last main trunk segment path1. NSmoothly exit the lane and connect to the exit path near the finish line. Smooth S-shaped / arc sections are visible at lane changes and curves, which are the transition paths generated in step 13. ti In step 14, point filling and smoothing were performed according to curvature constraints.

[0236] Figure 6 The example shown illustrates a complex scenario involving a large U-turn and ramp switching. The starting point is in area A, and the ending point is in area C. The route requires a large-radius U-turn on the main road and a ramp transition to the target area. At the road loop (near the U-shaped section), a curvature-constrained transition curve (such as a quadratic / cubic Bézier curve or a circular arc plus a transition curve) is used to meet the minimum turning radius R. min With the maximum lateral acceleration limit, step 14 can still maintain geometric continuity and curvature constraints when splicing multiple segments, and the output can be directly used for downstream velocity / timing planning.

[0237] Step 15: Integrate main road routes.

[0238] For the Path in step 14 main_aux By concatenating the paths, we obtain the PATH environment variable on the main road:

[0239]

[0240] Here, "+" indicates the concatenation of arrays.

[0241] Step 16: Patch the entire path.

[0242] The initial path of the mobile device is obtained by splicing together the access path, the main road path, and the outgoing path.

[0243] By sequentially concatenating the trajectories Path1, PATH, and Path2, a rough road trajectory PATH is obtained. PRE :

[0244] PATH PRE =Path1+PATH+Path2.

[0245] Step 17: Post-processing to optimize the path.

[0246] The initial path is optimized to obtain the target path for the mobile device.

[0247] Based on the lane boundary constraints and the curvature constraints of the car in different road segments, the PATH is... PRE Post-processing is performed to obtain the final desired path, PATH. RESULT .

[0248] Example 2:

[0249] like Figure 7As shown, this embodiment provides a path planning apparatus for executing the above-described path planning method, including:

[0250] The segmentation module 701 is used to acquire the target map and divide the target map into multiple sub-regions. The multiple sub-regions include a starting region and an ending region. The starting region is the region that includes the starting position of the movable device, and the ending region is the region that includes the ending position of the movable device.

[0251] The first determining module 702, connected to the dividing module 701, is used to determine the access point in the starting area and the exit point in the ending area. The access point is the path connection position from the starting point to the main road in the starting area, and the exit point is the path connection position from the main road to the ending point in the ending area.

[0252] The second determining module 703 is connected to the first determining module 702 and is used to determine the access path between the starting point and the access point, and the exit path between the exit point and the end point using a hybrid obstacle avoidance algorithm. The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. The hybrid obstacle avoidance algorithm is used to generate a continuous drivable path in the starting area and the end area that satisfies vehicle dynamics constraints and avoids obstacles.

[0253] The third determining module 704 is connected to the second determining module 703 and is used to determine the main road path between the access point and the exit point using the shortest path search algorithm. The shortest path search algorithm is used to determine the shortest path in the main road network, which is the set of main road segments identified in the road topology of the target map.

[0254] The fourth determining module 705, connected to the third determining module 704, is used to determine the target path of the mobile device based on the access path, the main road path, and the outgoing path.

[0255] Optionally, the first determining module 702 includes:

[0256] The first acquisition unit is used to acquire information on the main roads within the starting area, the first heading angle of the mobile device at the starting point, and the starting position. The main road information includes all main road segments within the starting area.

[0257] The first calculation unit is used to calculate the Euclidean distance between the starting point and the nearest point of each main road segment;

[0258] The second calculation unit is used to calculate the heading difference between the first heading angle and the heading angle of each main road segment;

[0259] The first determining unit is used to determine the target main road segment in the starting area based on the Euclidean distance and heading difference corresponding to each main road segment;

[0260] The second determining unit is used to determine the nearest point from the starting point to the target main road segment as the access point.

[0261] Optionally, the second determining module 703 includes:

[0262] The first creation unit is used to create a state space of the starting area based on obstacle information in the starting area, the positions of the starting point and the access point, the first heading angle of the starting point and the second heading angle of the access point. The state space is used to characterize all possible motion states of the mobile device in the starting area.

[0263] The third calculation unit is used to calculate the driving cost of each node in the state space based on the kinematic constraints and obstacle distribution of the mobile device.

[0264] The planning unit is used to perform path search in the state space based on the travel cost of each node and a search strategy based on a heuristic cost function, generating an access path between the starting point and the access point that simultaneously satisfies collision-free and kinematic constraints.

[0265] Optionally, the aforementioned third calculation unit specifically includes:

[0266] The first calculation subunit is used to calculate the position coordinates and heading angle of the potential next node based on the position coordinates and heading angle of the current node, combined with preset steering angle and driving direction parameters, and by applying the kinematic equations of the movable device.

[0267] The second calculation subunit is used to perform obstacle collision detection for each potential next node obtained from the calculation, and to filter out feasible nodes without collisions.

[0268] The third calculation subunit is used to calculate the incremental travel cost and heuristic cost from the current node to the feasible node for each feasible node. The incremental travel cost includes at least the travel distance cost, travel direction switching cost, steering angle cost, and steering angle change cost.

[0269] The fourth calculation subunit is used to add the cumulative cost of the current node to the travel cost increment to obtain the cumulative cost of the feasible node.

[0270] The determination sub-unit is used to determine the travel cost of a feasible node based on the cumulative cost and heuristic cost of the feasible node.

[0271] Among them, the travel distance cost is proportional to the travel step length, and an additional penalty coefficient is applied when traveling backward; the travel direction switching cost is generated when switching between forward and backward; the steering angle cost is proportional to the absolute value of the steering angle; and the steering angle change cost is proportional to the change in the current steering angle relative to the steering angle of the previous state.

[0272] Optionally, the fourth determining module 705 is specifically used to: calculate the shortest path between the access point and the exit point in the main road network based on a parallel shortest path search algorithm.

[0273] In this embodiment, different planning algorithms can be used for different areas. A hybrid obstacle avoidance algorithm is used in the starting and ending areas, while the shortest path search is used for the main road network. The hybrid obstacle avoidance algorithm ensures that the access and exit paths can effectively avoid obstacles while meeting the vehicle's dynamic constraints, thus achieving path feasibility and safety. The use of the shortest path search algorithm on the main roads simplifies path selection and improves the overall traffic efficiency. The target path is obtained by combining the three paths, thereby optimizing the driving path while meeting the driving needs of the mobile device.

[0274] Example 3:

[0275] Figure 8 A schematic diagram of the hardware structure of the electronic device provided in an embodiment of this application is shown.

[0276] An electronic device may include a processor 801 and a memory 802 storing computer program instructions.

[0277] Specifically, the processor 801 may include a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits that can be configured to implement the embodiments of this application.

[0278] Memory 802 may include mass storage for data or instructions. For example, and not limitingly, memory 802 may include a hard disk drive (HDD), floppy disk drive, flash memory, optical disk, magneto-optical disk, magnetic tape, or Universal Serial Bus (USB) drive, or a combination of two or more of these. Where appropriate, memory 802 may include removable or non-removable (or fixed) media. Where appropriate, memory 802 may be internal or external to the integrated gateway disaster recovery device. In a particular embodiment, memory 802 is non-volatile solid-state memory.

[0279] Memory may include read-only memory (ROM), random access memory (RAM), disk storage media devices, optical storage media devices, flash memory devices, and electrical, optical, or other physical / tangible memory storage devices. Therefore, typically, memory includes one or more tangible (non-transitory) computer-readable storage media (e.g., memory devices) encoded with software including computer-executable instructions, and when the software is executed (e.g., by one or more processors), it is operable to perform the operations described with reference to the methods according to one aspect of this disclosure.

[0280] The processor 801 implements any of the path planning methods described in the above embodiments by reading and executing computer program instructions stored in the memory 802.

[0281] In one example, the electronic device may also include a communication interface 803 and a bus 804. For example, Figure 8 As shown, the processor 801, memory 802, and path planning interface 803 are connected through bus 804 and communicate with each other.

[0282] The communication interface 803 is mainly used to realize communication between various modules, devices, units and / or equipment in the embodiments of this application.

[0283] Bus 804 includes hardware, software, or both, that couples components of an online data traffic metering device together. For example, and not limitingly, the bus may include an Accelerated Graphics Port (AGP) or other graphics bus, an Enhanced Industry Standard Architecture (EISA) bus, a Front Side Bus (FSB), HyperTransport (HT) interconnect, an Industry Standard Architecture (ISA) bus, an Infinite Bandwidth Interconnect, a Low Pin Count (LPC) bus, a memory bus, a Microchannel Architecture (MCA) bus, a Peripheral Component Interconnect (PCI) bus, a PCI-Express (PCI-X) bus, a Serial Advanced Technology Attachment (SATA) bus, a Video Electronics Standards Association Local (VLB) bus, or other suitable buses, or combinations of two or more of these. Where appropriate, bus 804 may include one or more buses. Although specific buses are described and illustrated in embodiments of this application, any suitable bus or interconnect is contemplated herein.

[0284] Furthermore, in conjunction with the path planning methods in the above embodiments, this application embodiment can provide a computer storage medium for implementation. The computer storage medium stores computer program instructions; when these computer program instructions are executed by a processor, they implement any of the path planning methods in the above embodiments.

[0285] It should be clarified that this application is not limited to the specific configurations and processes described above and shown in the figures. For the sake of brevity, detailed descriptions of known methods are omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method process of this application is not limited to the specific steps described and shown. Those skilled in the art can make various changes, modifications, and additions, or change the order of steps, after understanding the spirit of this application.

[0286] The functional blocks shown in the above block diagram can be implemented as hardware, software, firmware, or a combination thereof. When implemented in hardware, they can be, for example, electronic circuits, application-specific integrated circuits (ASICs), appropriate firmware, plug-ins, function cards, etc. When implemented in software, the elements of this application are programs or code segments used to perform the required tasks. The programs or code segments can be stored on a machine-readable medium or transmitted over a transmission medium or path-planned link via data signals carried on a carrier wave. "Machine-readable medium" can include any medium capable of storing or transmitting information. Examples of machine-readable media include electronic circuits, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, CD-ROMs, optical disks, hard disks, fiber optic media, radio frequency (RF) links, etc. Code segments can be downloaded via computer networks such as the Internet, intranets, etc.

[0287] It should also be noted that the exemplary embodiments mentioned in this application describe methods or systems based on a series of steps or apparatus. However, this application is not limited to the order of the above steps; that is, the steps can be performed in the order mentioned in the embodiments, or in a different order, or several steps can be performed simultaneously.

[0288] The aspects of this disclosure have been described above with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this disclosure. It should be understood that each block in the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that these instructions, executable via the processor of the computer or other programmable data processing apparatus, enable the implementation of the functions / actions specified in one or more blocks of the flowchart illustrations and / or block diagrams. Such a processor can be, but is not limited to, a general-purpose processor, a special-purpose processor, a special application processor, or a field-programmable logic circuit. It is also understood that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can also be implemented by special-purpose hardware performing the specified functions or actions, or can be implemented by a combination of special-purpose hardware and computer instructions.

[0289] The above are merely specific embodiments of this application. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. It should be understood that the protection scope of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the protection scope of this application.

Claims

1. A path planning method, characterized in that, Applied to a mobile device, the method includes: Obtain the target map and divide the target map into multiple sub-regions, the multiple sub-regions including a starting region and an ending region, the starting region being the region including the starting position of the movable device, and the ending region being the region including the ending position of the movable device; Determine the access point in the starting area and the exit point in the ending area. The access point is the path connection location from the starting point to the main road in the starting area, and the exit point is the path connection location from the main road to the ending point in the ending area. Using a hybrid obstacle avoidance algorithm, the access path between the starting point and the access point is determined, and the exit path between the exit point and the end point is determined. The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. The hybrid obstacle avoidance algorithm is used to generate continuous drivable paths that satisfy vehicle dynamics constraints and avoid obstacles in the starting point region and the end point region. The shortest path search algorithm is used to determine the main road path between the access point and the outgoing point. The shortest path search algorithm is used to determine the shortest path in the main road network, which is the set of main road segments identified in the road topology of the target map. The target path of the mobile device is determined based on the access path, the main road path, and the outgoing path.

2. The method according to claim 1, characterized in that, The determination of the access point in the starting area specifically includes: Acquire the main road information within the starting area, the first heading angle of the movable device at the starting point, and the starting position. The main road information includes all main road segments within the starting area. Calculate the Euclidean distance between the starting point and the nearest point on each main road segment; Calculate the heading difference between the first heading angle and the heading angle of each main road segment; The target main road segment in the starting area is determined based on the Euclidean distance and the heading difference corresponding to each main road segment; The nearest point from the starting point to the target main road segment is determined as the access point.

3. The method according to claim 1, characterized in that, The process of using a hybrid obstacle avoidance algorithm to determine the access path between the starting point and the access point specifically includes: Based on the obstacle information in the starting area, the positions of the starting point and the access point, the first heading angle of the starting point and the second heading angle of the access point, a state space of the starting area is created. The state space is used to characterize all possible motion states of the mobile device in the starting area. The driving cost of each node in the state space is calculated based on the kinematic constraints of the mobile device and the distribution of obstacles. Based on the travel cost of each node, a search strategy based on a heuristic cost function is adopted to perform path search in the state space, generating an access path between the starting point and the access point that simultaneously satisfies collision-free and kinematic constraints.

4. The method according to claim 3, characterized in that, The step of calculating the travel cost of each node in the state space based on the kinematic constraints of the movable device and the obstacle distribution specifically includes: Based on the position coordinates and heading angle of the current node, combined with the preset steering angle and driving direction parameters, the kinematic equations of the movable device are applied to calculate the position coordinates and heading angle of the potential next node. For each potential next node calculated, obstacle collision detection is performed to filter out feasible nodes without collisions; For each feasible node, calculate the incremental travel cost and heuristic cost from the current node to the feasible node. The incremental travel cost includes at least the travel distance cost, travel direction switching cost, steering angle cost, and steering angle change cost. The cumulative cost of the current node is added to the travel cost increment to obtain the cumulative cost of the feasible node; The travel cost of a feasible node is determined based on the cumulative cost and heuristic cost of the feasible node.

5. The method according to claim 1, characterized in that, The process of using the shortest path search algorithm to determine the main path between the access point and the outgoing point specifically includes: In the main road network, the shortest path between the access point and the outgoing point is calculated based on a parallel shortest path search algorithm.

6. A path planning device, characterized in that, Applied to a mobile device, the device comprising: The segmentation module is used to acquire the target map and divide the target map into multiple sub-regions. The multiple sub-regions include a starting region and an ending region. The starting region is the region that includes the starting position of the movable device, and the ending region is the region that includes the ending position of the movable device. The first determining module, connected to the dividing module, is used to determine the access point in the starting area and the exit point in the ending area. The access point is the path connection position from the starting point to the main road in the starting area, and the exit point is the path connection position from the main road to the ending point in the ending area. The second determining module, connected to the first determining module, is used to determine the access path between the starting point and the access point, and the exit path between the exit point and the end point using a hybrid obstacle avoidance algorithm. The hybrid obstacle avoidance algorithm is a path planning algorithm that combines a heuristic search algorithm with vehicle dynamics constraints. The hybrid obstacle avoidance algorithm is used to generate continuous drivable paths that satisfy vehicle dynamics constraints and avoid obstacles in the starting point region and the end point region. The third determining module, connected to the second determining module, is used to determine the main road path between the access point and the outgoing point using a shortest path search algorithm. The shortest path search algorithm is used to determine the shortest path in the main road network, where the main road network is a set of main road segments identified in the road topology of the target map. The fourth determining module, connected to the third determining module, is used to determine the target path of the mobile device based on the access path, the main road path, and the outgoing path.

7. The apparatus according to claim 6, characterized in that, The first determining module includes: The first acquisition unit is used to acquire the main road information within the starting area, the first heading angle of the movable device at the starting point and the starting position, wherein the main road information includes all main road segments within the starting area; The first calculation unit is used to calculate the Euclidean distance between the starting point and the nearest point of each main road segment; The second calculation unit is used to calculate the heading difference between the first heading angle and the heading angle of each main road segment; The first determining unit is used to determine the target main road segment in the starting area based on the Euclidean distance and the heading difference corresponding to each main road segment; The second determining unit is used to determine the nearest point from the starting point to the target main road segment as the access point.

8. The apparatus according to claim 6, characterized in that, The second determining module includes: The first creation unit is used to create a state space of the starting area based on obstacle information in the starting area, the positions of the starting point and the access point, the first heading angle of the starting point and the second heading angle of the access point. The state space is used to characterize all possible motion states of the mobile device in the starting area. The third calculation unit is used to calculate the driving cost of each node in the state space based on the kinematic constraints and obstacle distribution of the mobile device. The first path planning unit is used to perform path search in the state space based on the travel cost of each node and a search strategy based on a heuristic cost function, so as to generate an access path between the starting point and the access point that simultaneously satisfies collision-free and kinematic constraints.

9. An electronic device, characterized in that, It includes a processor, a memory, and a program or instructions stored in the memory and executable on the processor, wherein the program or instructions, when executed by the processor, implement the path planning method as described in any one of claims 1-5.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer program instructions that, when executed by a processor, implement the path planning method as described in any one of claims 1-5.

Images