Heuristic search-based parking trajectory planning methods, systems, electronic devices, and software products

Abstract

This application provides a parking trajectory planning method based on heuristic search. The method includes: preprocessing a grid map input from an obstacle fusion module of a vehicle waiting to park to obtain an effective obstacle set; calculating the distance from each free grid cell to the nearest obstacle based on the preprocessed grid map and saving the calculated distance to the grid cell of the grid, thereby forming an obstacle distance map; determining the parking scene level based on environmental information so that planning parameters corresponding to the determined scene level are used during trajectory generation; performing trajectory search using a hybrid A* algorithm, with the current position of the vehicle waiting to park as the starting point and the parking termination posture as the target point, to generate an initial trajectory; and optimizing the generated initial trajectory based on an objective function and constraints to adjust the initial trajectory into a smooth trajectory. Corresponding systems and program products are also provided.

Description

Technical Field

[0001] This application relates to vehicle-assisted driving technology, and more specifically, to parking trajectory planning based on heuristic search. Background Technology

[0002] With the rapid growth of car ownership, the problems of traffic congestion on urban roads and the scarcity of parking space resources are becoming increasingly prominent, leading to a continuous increase in the demand for automation and intelligence in the vehicle parking process. As an important application scenario of intelligent driving technology, automatic parking systems aim to help drivers safely and efficiently complete parking operations in narrow and complex parking environments through technological means, reducing the difficulty of driving operations and improving parking safety and convenience.

[0003] In automated parking systems, trajectory planning technology is a core component. Its goal is to generate parking trajectories that meet safety, feasibility, and smoothness requirements based on information such as vehicle kinematic constraints and the distribution of environmental obstacles, providing a reliable basis for vehicle control. Trajectory search and generation are key steps in trajectory planning, directly impacting the overall performance and operational efficiency of the parking system.

[0004] Currently, in the field of automated parking trajectory planning, traditional trajectory solving methods are mostly based on geometric rule algorithms. Although these methods can quickly calculate parking paths in conventional parking scenarios and meet basic parking requirements, they still have many shortcomings in practical applications, such as: 1. For complex parking scenarios such as dead-end parking spaces and extremely narrow parking spaces, it is not possible to effectively generate feasible trajectories that meet the vehicle kinematic constraints, resulting in a low trajectory solution success rate; 2. The trajectory generation is not well adapted to the actual driving needs of the vehicle. The solved trajectory often has problems such as sudden curvature changes and inconsistent driving logic, which makes it difficult to meet the smoothness requirements of vehicle control and affects the stability of parking operations. 3. The algorithm has poor flexibility and cannot adjust the search strategy according to the characteristics of different parking scenarios. It is difficult to balance the efficiency of trajectory solving and the feasibility of the path. In complex scenarios, it is prone to problems such as search lag and trajectory solving failure.

[0005] In summary, current automated parking trajectory planning technology still has room for improvement in terms of adaptability to complex scenarios, trajectory feasibility, and solution efficiency, and is insufficient to fully meet the automated and intelligent parking needs in complex urban parking environments. Summary of the Invention

[0006] According to some embodiments of this application, a parking trajectory planning method based on heuristic search is provided to at least address one of the above-mentioned deficiencies.

[0007] According to one aspect of this application, a parking trajectory planning method based on heuristic search is provided. The method includes preprocessing a grid map input from an obstacle fusion module of a vehicle waiting to park to obtain an effective obstacle set; calculating the distance from each free grid cell to the nearest obstacle based on the preprocessed grid map and saving the calculated distance to the grid cell of the grid, thereby forming an obstacle distance map; determining the parking scene level based on environmental information so that planning parameters corresponding to the determined scene level are used during trajectory generation; performing trajectory search using a hybrid A* algorithm, starting from the current position of the vehicle waiting to park and targeting the parking termination posture, to generate an initial trajectory; and optimizing the generated initial trajectory based on an objective function and constraints to adjust the initial trajectory into a smooth trajectory.

[0008] According to another aspect of the application, a parking trajectory planning system based on heuristic search is provided, including an obstacle detection module for detecting obstacles around a vehicle waiting to be parked and forming a grid map based on the detected obstacles, the grid map including drivable grids, obstacle grids, and unknown grids; and a trajectory planner for receiving the grid map transmitted by the obstacle detection module, the trajectory planner being configured to perform the parking trajectory planning method based on heuristic search described herein.

[0009] An electronic device is also provided, the electronic device including a memory and a processor, the memory including instructions, the processor being configured to execute the instructions to implement the heuristic search-based parking trajectory planning method described herein.

[0010] A program product is also provided, characterized in that the program product includes instructions that, when executed, implement the heuristic search-based parking trajectory planning method described herein. Attached Figure Description

[0011] The embodiments of this application will be described in detail below with reference to the accompanying drawings, so that this application can be more fully understood, wherein: Figure 1 This is a diagram showing the vehicles waiting to park and the parking spaces; Figure 2 This is a flowchart of a parking trajectory planning method based on heuristic search, as illustrated in this application. Figure 3 Steps Figure 2 An exemplary process of S206; Figure 4 This is a process performed according to some examples of this application when the obstacle distribution is sparse or the current search node is close to the target point; Figure 5This is a schematic diagram of the structure of a parking trajectory planning system based on heuristic search according to some embodiments of this application. Detailed Implementation

[0012] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the implementation methods of this application will be clearly and completely described below with reference to the accompanying drawings. It should be noted that the described implementation methods are only a part of the implementation methods of the technical solutions of this application, and not all of them. All other implementation methods obtained by those skilled in the art based on the implementation methods described in this application document without creative effort are covered by the protection scope of this application.

[0013] Figure 1 This is a schematic diagram of a vehicle waiting to be parked and a parking space. The vehicle 10 is following a planned path to park in the parking space 20. For example, the vehicle 10 is equipped with a parking sensor group 100, a positioning module 102, a parking controller 104, and a memory 106. The parking sensor group 100 may include surround-view cameras, ultrasonic radar, millimeter-wave radar, etc., for sensing objects around the vehicle, including obstacles, lane lines, etc., and providing this sensing data to the parking controller 104. The positioning module 102 includes a navigation system, wheel speed force sensors, steering wheel angle sensors, etc., for providing the parking controller 104 with data such as vehicle position and speed required for the parking process. The memory 106 stores data such as the vehicle's dimensions and dynamic model, so that the parking controller 104 can use it as needed during the parking process.

[0014] Figure 2 This is a flowchart of a parking trajectory planning method based on heuristic search, as exemplified in this application.

[0015] In step S200, the grid map input from the obstacle fusion module of the waiting vehicle is preprocessed to obtain a valid set of obstacles. In some examples, the obstacle fusion module processes obstacle data input from the parking sensor group 100 to form a grid map, which includes drivable grids, obstacle grids, and unknown grids. In other examples, the parking sensor group 100 is part of the obstacle fusion module. The grid map formed by the obstacle fusion module (i.e., the original grid map) usually contains noise caused by factors such as sensor measurement errors, environmental interference, and false detections of dynamic targets. If these noises are not removed and trajectory planning is performed directly based on the original grid map, it is easy for the algorithm to generate unnecessary avoidance actions, or even cause planning failure. According to this embodiment, the grid map input from the obstacle fusion module of the waiting vehicle is preprocessed to filter out obstacle noises that affect planning, that is, to eliminate invalid obstacle information in the grid map and retain the valid set of obstacles in the grid map. As an example, invalid information in the raster map can be removed through spatial connectivity analysis, size threshold filtering, and statistical filtering. Then, the valid obstacles in the raster map, after the invalid information has been removed, are expanded to fit the dimensions of the vehicle waiting to park and to allow for safety redundancy. By expanding the obstacles in the raster map, for example, by expanding them outwards by several rings of grid space, the planned trajectory can be forced to maintain a certain distance from the obstacles, ensuring safety redundancy for vehicle movement.

[0016] In step S202, based on the preprocessed grid map, the distance from each free grid cell to the nearest obstacle is calculated and the calculated distance is saved to the grid cell of the grid cell, thereby forming an obstacle distance map. A free grid cell refers to the drivable area of ​​the vehicle in the grid map. The grid map also includes occupied grid cells, which correspond to obstacle areas. Calculating their distances to obstacles is meaningless for trajectory planning; therefore, only the distances between free grid cells and obstacles need to be calculated. In this example, the Euclidean distance between each free grid cell and the nearest obstacle is calculated and stored in the corresponding grid cell. Therefore, based on this distance information, collision detection can be quickly completed during the trajectory search phase, and obstacle distance constraints can be introduced into the objective function during the trajectory optimization phase, thereby achieving dynamic safety margin control between the trajectory and obstacles. In another example, the distance between each free grid cell and the nearest obstacle can also be calculated using, for example, a breadth-first search traversal algorithm.

[0017] In step S204, the parking scenario level is determined based on environmental information so that planning parameters corresponding to the determined scenario level can be used during trajectory generation.

[0018] For example, environmental information includes the size and geometry of parking spaces, the number and distribution of surrounding obstacles, the openness of available parking space, the accessibility of the driving area, and the presence of dynamic targets. Parking space-related information (such as parking space size and geometry, and the openness of parking space) can be obtained in some cases through sensors on the vehicle waiting to park, or in others pre-stored in the vehicle, for example, by the parking management system interacting with the vehicle and transmitting relevant data. The number and distribution of surrounding obstacles can be obtained through sensors on the vehicle, such as radar sensors; or determined from a grid map input by the obstacle fusion module; or a combination of both. The presence of dynamic targets in the driving area can also be obtained through sensor components such as radar or cameras, and the accessibility of the driving area can be determined by combining sensor data from radar or cameras, map data, etc. Weighting coefficients can be set for the above features in the environmental information, thereby classifying the parking scene around the vehicle as a low-complexity, medium-complexity, or high-complexity scene based on these features. Low-complexity scenes include open scenes, while high-complexity scenes include narrow scenes.

[0019] Different planning parameters are used in trajectory planning for different parking scenarios. Planning parameters corresponding to low-complexity scenarios include a sampling density below a sampling threshold and a search step size above a preset search step size threshold. For example, the preset sampling density threshold and search step size threshold can be empirical values ​​calibrated based on the parking scenario. In low-complexity parking scenarios, during path planning, the planner may use a sampling density below the preset sampling density threshold to reduce the number of trajectory points and improve computational efficiency; simultaneously, it may use a search step size above the preset search step size threshold to expand the single search range and accelerate the path search process. Planning parameters corresponding to medium-complexity scenarios include obstacle safety boundaries increased to corresponding preset values ​​and search step sizes decreased to corresponding preset values. For example, the preset values ​​for obstacle safety boundaries and search step sizes can both be empirical values ​​calibrated based on the parking scenario. In medium-complexity parking scenarios, during path planning, the planner may increase the obstacle safety boundary to a preset value to improve parking safety; simultaneously, it may decrease the search step size to a preset value to improve path search accuracy while maintaining planning efficiency, thus achieving a reasonable balance between safety and planning efficiency. For high-complexity scenarios, the planning parameters include the iteration increment and the trajectory curvature change constraint threshold. The iteration increment improves trajectory optimization accuracy, and the trajectory curvature change constraint threshold restricts trajectory curvature changes. For example, both the iteration increment and the trajectory curvature change constraint threshold can be empirical values ​​calibrated based on the parking scenario. In high-complexity parking scenarios, during path planning, the planner may use an increased iteration count for trajectory optimization to improve trajectory solution accuracy and feasibility; simultaneously, the trajectory curvature change constraint threshold strictly restricts trajectory curvature changes to ensure the trajectory remains continuous and controllable within a confined space, ensuring the final generated parking trajectory has good executability and safety.

[0020] In step S206, the Hybrid A Star algorithm is used to search for a trajectory, starting from the current position of the vehicle waiting to be parked and targeting the parking termination posture, in order to generate an initial trajectory.

[0021] In the example of this application, the search space is established based on the preprocessed grid map, using either the current position of the vehicle waiting to be parked or its parking termination posture as the search starting point and the other as the target point. For example, in a forward search, the current position of the vehicle is the starting point, and the parking termination posture is the target point; while in a reverse search, the parking termination posture is the starting point, and the current position of the vehicle is the ending point. Whether to use forward or reverse search in the hybrid A* algorithm search process can be determined, for example, based on the parking scenario level. For instance, forward search is preferred for low-complexity scenarios, while reverse search is preferred for high-complexity scenarios. It should be noted that forward and reverse searches can be switched for different scenarios, as long as the time consumption is optimized while ensuring the success rate of path planning.

[0022] Figure 3 This is an exemplary process of step S206, which is applicable to both forward and reverse searches.

[0023] In step S300, a search space is established based on the preprocessed grid map, using either the current position of the vehicle waiting to be parked or its parking termination posture as the search starting point and the other as the target point. For example, the search space is established based on a discretized grid map.

[0024] In step S302, during the state expansion process of the node search using the A* algorithm in the search space, kinematic model constraints of the vehicle waiting to park are introduced. These constraints include the minimum turning radius, maximum rate of change of steering angle, and nonholonomic constraints of the vehicle. Kinematic model constraints are inherent physical properties of the vehicle itself. During the hybrid A* trajectory search, these inherent parameters of the vehicle's constraints are written into the constraint algorithm to limit the node state expansion and ensure that the trajectory can be actually executed by the vehicle. The minimum turning radius is the minimum circular radius that the vehicle can achieve when turning at full steering. In the hybrid A* trajectory search, the trajectory turning radius cannot be less than this value; otherwise, the vehicle cannot turn and track. The maximum rate of change of steering angle is the maximum rate of change of the steering wheel angle per unit time. In the hybrid A* trajectory search, it is necessary to limit abrupt changes in the steering angle of the trajectory to avoid "sudden twists" in the trajectory that the vehicle's steering mechanism cannot keep up with. Nonholonomic constraints refer to situations where a vehicle cannot move laterally, its direction of motion is constrained by the orientation of the front wheels, and its velocity direction is always along the longitudinal direction of the vehicle body. In hybrid A* trajectory search, the trajectory planning algorithm must be prohibited from generating paths that the vehicle cannot achieve, such as lateral or oblique movements.

[0025] In step S304, candidate nodes in the search process are evaluated using a heuristic cost function, which includes path cost, heuristic distance cost, and trajectory smoothing cost. The path cost records the cumulative actual travel cost from the search starting point to the current candidate node, summing the path cost of each node expansion step, such as travel distance, steering angle cost, and energy consumption cost, to ensure the actual executability of the trajectory and avoid seemingly short but actually unexecutable paths. The heuristic distance cost estimates the future cost prediction from the current candidate node to the target point, using Euclidean distance, Manhattan distance, or vehicle attitude adaptation distance, and can also be calculated by combining position and heading angle, guiding the algorithm to search efficiently towards the target point, avoiding meaningless global traversal, and improving search efficiency. The trajectory smoothing cost measures the smoothness of the current candidate trajectory, quantifies the curvature of the trajectory, and calculates the rate of change of heading angle, curvature change, or second derivative of the trajectory segment for adjacent nodes.

[0026] For example, three types of cost functions are constructed: path cost, heuristic distance cost, and trajectory smoothing cost. The total cost of candidate nodes is calculated according to formula (1) based on the weighted fusion rule: f(n)= w1×g(n)+w2×h(n)+w3×s(n) (1) Where w1, w2, and w3 are preset weight coefficients, g(n) is the path cost, h(n) is the heuristic distance cost, and s(n) is the trajectory smoothing cost.

[0027] The candidate node with the lowest total cost is selected from the open list for state expansion. During the expansion process, collision detection is performed by combining vehicle kinematics model constraints and obstacle distance map. Only feasible nodes are retained and their costs are updated. When a node meets the target point tolerance condition, the search is terminated and the initial parking trajectory is generated backtracked. Specifically, in one embodiment, the path cost is accumulated by the cost of driving distance and steering angle, the heuristic distance cost is fused by the difference between the Euclidean distance of the position and the heading angle, and the trajectory smoothing cost is accumulated by the rate of change of heading angle and the change of curvature. As an example, w1, w2, and w3 are 0.5, 0.3, and 0.2, respectively, to balance the actual executability, search efficiency, and smoothness of the trajectory.

[0028] In step S306, during each node state expansion process, collision detection of candidate trajectories is performed based on the obstacle distance map to ensure that the candidate trajectories avoid the obstacles. Specifically, during each node state expansion process, collision detection of candidate trajectories is performed based on the obstacle distance map to ensure that the candidate trajectories avoid the obstacles; wherein, the obstacle distance map is constructed based on a preprocessed grid map and is used to represent the distance information between each location in the map and the nearest obstacle; during collision detection, the vehicle body size and outline of the vehicle waiting to park are considered to determine whether the vehicle coverage area corresponding to the candidate trajectory segment intrudes into the obstacle area or the distance between it and the obstacle is less than a preset safe distance; nodes corresponding to candidate trajectories with collision risk are discarded, and only nodes without collision risk are retained to participate in subsequent heuristic cost function evaluation and state expansion.

[0029] In step S308, when the search reaches the vicinity of the target point and meets the preset tolerance conditions, the search process of the hybrid A-Star algorithm is terminated, and a feasible trajectory from the starting point to the target point is generated by backtracking the search path, serving as the initial trajectory. Specifically, when the search extends to the vicinity of the target point, and the positional deviation and heading angle deviation between the current node and the parking termination posture both meet the preset tolerance conditions, the search process of the hybrid A-Star algorithm is terminated; wherein, the preset tolerance conditions include the positional coordinate deviation being less than a preset position threshold and the heading angle deviation being less than a preset angle threshold. After determining that the termination conditions are met, starting from the target node that meets the conditions, the path is backtracked according to the parent node pointers stored in each node, and the state node sequence from the target node to the search starting point is obtained sequentially. The backtracked state node sequence is arranged in reverse chronological order to generate a continuous, collision-free feasible trajectory from the starting point to the target point that meets the vehicle kinematic constraints, serving as the initial trajectory for the parking path planning.

[0030] Back Figure 2 After generating the initial trajectory, the process proceeds to step S208, where the generated initial trajectory is optimized based on the objective function and constraints to adjust it into a smooth trajectory.

[0031] Specifically, a trajectory optimization model is constructed using the trajectory nodes of the initial trajectory as optimization variables. This model includes the objective functions and constraints. The objective functions include a smoothness objective function, a safety objective function, and a length and efficiency objective function. The constraints include kinematic and dynamic constraints for the vehicle waiting to park. The Operator Splitting Quadratic Programming (OSQP) algorithm is used to solve the optimization model to adjust the initial trajectory into a smooth trajectory that meets the constraints. The smoothness objective function ensures the spatial continuity and smoothness of the trajectory by minimizing indices such as trajectory curvature, rate of change of heading angle, or second derivative. The safety objective function, combined with an obstacle distance map, constrains the distance between trajectory points and obstacles to be no less than a set safety distance. The length and efficiency objective functions avoid unnecessary detours, minimize trajectory length, and improve parking efficiency.

[0032] In this way, the optimized trajectory eliminates redundant polyline segments, ensuring the continuity of curvature and the controllability of the trajectory, allowing the vehicle control module to perform parking operations stably and smoothly.

[0033] According to the embodiments of this application, in the execution Figure 2 The method shown can be executed when obstacles are detected to be sparsely distributed or the current search node is close to the target point. Figure 4 The process is illustrated. For example, during the node state expansion process, if the hybrid A* algorithm detects that the obstacle distribution is sparse based on the grid map and obstacle distance map, it can execute... Figure 4 The process shown; or, in the process of node state expansion using the hybrid A* algorithm, if based on the grid map and obstacle distance map, and determining the current search node's proximity to the target point according to the position distance and heading difference between the current node's coordinates and the target parking posture, the following can be executed: Figure 4 The process is shown.

[0034] like Figure 4As shown, in step S400, based on the kinematic model of the vehicle to be parked and the attitude information of the target point, a geometric curve satisfying nonholonomic constraints is constructed. This geometric curve includes the Dubins curve (proposed by Dubins in 1957) or the Reeds-Shepp curve (proposed by J.A. Reeds and L.A. Shepp in 1990). When constructing the geometric curve, the constraints are the underlying kinematic constraints of the vehicle to be parked, mainly including the vehicle's minimum turning radius, maximum rate of change of steering angle, and nonholonomic constraints, to ensure that the constructed geometric curve is the actual drivable path of the vehicle. In step S402, with the current search node as the starting point and the target point as the ending point, the corresponding geometric connection trajectory segment is calculated. If it is a forward search, the corresponding geometric connection trajectory segment is calculated with the current search node (i.e., the current node in the forward search process) as the starting point and the parking termination posture (i.e., the forward search target point) as the ending point. If it is a reverse search, the corresponding geometric connection trajectory segment is calculated with the current search node (i.e., the current node in the reverse search process) as the starting point and the current position of the vehicle waiting to be parked (i.e., the reverse search target point) as the ending point. In step S404, the feasibility of the geometric connection trajectory segment is checked. For example, the feasibility check includes kinematic constraint detection, collision detection, and trajectory quality assessment. Kinematic constraint detection is used to confirm that the turning radius and curvature change of the geometric connection trajectory segment are within the achievable range of the vehicle waiting to be parked. Collision detection is used to check, in conjunction with the obstacle distance map, that all discrete points on the geometric connection trajectory segment do not overlap with obstacles. Trajectory quality assessment is used to confirm that the length and curvature continuity of the geometric connection trajectory segment meet the requirements of subsequent tracking control. If all feasibility checks pass, proceed to step S406; if the feasibility checks fail, proceed to step S408. In step S406, after confirming that the geometric curve connection is successful, the current search node and the target point are directly connected through the geometric curve, a complete path is generated by backtracking, and the entire trajectory search process is terminated. In step S408, the current geometric connection attempt is abandoned, and the search continues using the conventional extension strategy of the hybrid A* algorithm.

[0035] In summary, according to the embodiments of this application, when executing the parking trajectory planning method shown in Figure 2, the hybrid A* algorithm is first used to search for a trajectory, starting from the current position of the vehicle waiting to be parked and targeting the parking termination posture. Specifically, a search space is established based on a preprocessed grid map, using either the current position of the vehicle waiting to be parked or the parking termination posture as the starting point and the other as the target point. During the node state expansion process in the search space, kinematic model constraints of the vehicle waiting to be parked are introduced, including the minimum turning radius, maximum rate of change of turning angle, and nonholonomic constraints. Candidate nodes in the search process are evaluated using a heuristic cost function that includes path cost, heuristic distance cost, and trajectory smoothing cost. In each step of node state expansion, collision detection is performed on the candidate trajectories based on the obstacle distance map to ensure that the candidate trajectories avoid obstacles. At the same time, during the search process executed by the hybrid A* algorithm, the distribution of obstacles around the current node can be determined based on the grid map and the obstacle distance map, and the positional distance and heading angle deviation between the current node coordinates and the target parking posture can be used to determine whether the current search node is close to the target point. When obstacles are detected to be sparsely distributed or the current search node is close to the target point, the geometric curve fast connection process shown in Figure 4 can be executed to improve the trajectory search efficiency.

[0036] Compared to traditional geometric rule-based algorithms, which are unable to adapt to complex parking scenarios such as dead-end parking spaces or extremely narrow parking spaces, the method according to the embodiments of this application employs a hybrid A* heuristic search algorithm. It combines parking scenario grading with the configuration of corresponding planning parameters for parking scenarios of varying complexity. Simultaneously, it supports both forward and reverse bidirectional search modes, allowing for adjustment of search priorities based on scenario requirements. This significantly improves the trajectory generation success rate in complex scenarios, solving the problem of traditional algorithms being unable to adapt to complex situations. Furthermore, the method according to the embodiments of this application follows vehicle kinematic constraints during trajectory generation, introducing nonholonomic constraints to ensure trajectory drivability. It also optimizes the objective function to ensure the trajectory meets the requirements of curvature continuity and attitude stability, adapting to vehicle control needs and solving the problem of traditional trajectories being disconnected from actual driving operations. Traditional geometric rule algorithms have fixed rules, making it impossible to adjust the calculation strategy according to the scenario. Maintaining the algorithm requires modifying the core rules, resulting in high costs and poor flexibility. The method according to the embodiments of this application adopts a modular design, which separates trajectory search, path optimization and constraint control into independent modules. The planning parameters (such as sampling density and search step size) can be flexibly adjusted according to the needs of the scenario without modifying the core algorithm logic, thus reducing maintenance costs. At the same time, it introduces efficient solvers such as OSQP, which balances the solution speed and trajectory quality, and solves the problems of low efficiency and poor robustness of traditional algorithms.

[0037] In summary, this application achieves high efficiency and reliability in parking trajectory planning in terms of complex scenario adaptation, trajectory feasibility, and algorithm flexibility through a scheme based on heuristic search, directed search, and scenario-based parameter configuration.

[0038] Figure 5 This is a schematic diagram of the structure of a parking trajectory planning system based on heuristic search according to some embodiments of this application. Figure 5 As shown, the parking trajectory planning system includes an obstacle detection module 50 and a trajectory planner 52. The obstacle detection module 50 detects obstacles around the vehicle to be parked and forms a grid map based on the detected obstacles. The grid map includes drivable grids, obstacle grids, and unknown grids. As an example, the obstacle detection module 50 includes a parking sensor group installed on the vehicle, such as a surround-view camera, ultrasonic radar, millimeter-wave radar, etc. The obstacle detection module 50 may also include a processing unit that processes the obstacle data input from the parking sensor group to form a grid map, which includes drivable grids, obstacle grids, and unknown grids. The planner 52 is configured to receive the grid map transmitted by the obstacle detection module and is also connected to other related components to receive information, such as the vehicle's positioning module. The planner 52 is configured to execute any of the heuristic search-based parking trajectory planning methods described herein. As an example, the planner 52 may be a parking system controller for the vehicle.

[0039] This application also provides an electronic device including a memory and a processor, the memory including instructions, the processor being configured to execute the instructions to implement any of the heuristic search-based parking trajectory planning methods described herein.

[0040] This application also provides a program product comprising instructions that, when executed, implement any of the heuristic search-based parking trajectory planning methods described herein.

[0041] The technical features in the various embodiments of this application can be combined with each other to form new implementation methods without departing from the spirit of this application and without conflicting with each other. Although specific embodiments of this application have been shown and described in detail to illustrate the principles of this application, it should be understood that this application can be implemented in other ways without departing from such principles.

Claims

1. A heuristic search based parking trajectory planning method, characterized in that, The method includes: The grid map input from the obstacle fusion module of the waiting vehicles is preprocessed to obtain an effective set of obstacles; Based on the preprocessed grid map, the distance from each free grid cell to the nearest obstacle is calculated and the calculated distance is saved to the grid cell of the grid, thereby forming an obstacle distance map; Based on environmental information, the parking scenario level is determined so that planning parameters corresponding to the determined scenario level can be used during trajectory generation. Using the hybrid A* algorithm, a trajectory search is performed with the current position of the vehicle waiting to be parked as the starting point and the parking termination posture as the target point, so as to generate an initial trajectory. The generated initial trajectory is optimized based on the objective function and constraints to adjust the initial trajectory into a smooth trajectory.

2. The method of claim 1, wherein, The preprocessing of the grid map input from the obstacle fusion module of the waiting vehicles includes: Invalid information in the raster map is removed through spatial connectivity analysis, size threshold filtering, and statistical filtering. The valid obstacles in the grid map, after the invalid information has been removed, are expanded to fit the dimensions of the vehicles waiting to park and to allow for safety redundancy.

3. The method of claim 1, wherein, The environmental information includes the size and geometry of the parking space, the number and distribution of surrounding obstacles, the openness of the available parking space, the accessibility of the driving area, and the presence of dynamic targets; and the parking scene level includes low complexity scene, medium complexity scene, and high complexity scene.

4. The method according to claim 3, characterized in that, The planning parameters corresponding to the low-complexity scenario include a sampling density below a sampling threshold and a search step size greater than a preset search step size threshold; the planning parameters corresponding to the medium-complexity scenario include an obstacle safety boundary increased to a corresponding preset value and a search step size decreased to a corresponding preset value; the planning parameters corresponding to the high-complexity scenario include an iteration increment and a trajectory curvature change constraint threshold, wherein the iteration increment is increased, and the trajectory curvature change constraint threshold is used to limit the trajectory curvature change.

5. The method according to claim 1, characterized in that, The process of using a hybrid A* algorithm to perform trajectory search, starting from the current position of the vehicle waiting to be parked and targeting the parking termination posture, includes: A search space is established based on the preprocessed grid map, with one of the current position of the vehicle waiting to be parked and the parking termination posture as the search starting point and the other as the target point. During the state expansion process of the search node in the search space, the kinematic model constraints of the waiting vehicle are introduced, wherein the constraints include the minimum turning radius, the maximum rate of change of turning angle, and nonholonomic constraints of the waiting vehicle. Candidate nodes in the search process are evaluated using a heuristic cost function, which includes path cost, heuristic distance cost, and trajectory smoothing cost. During each node state expansion process, collision detection of candidate trajectories is performed based on the obstacle distance map to ensure that the candidate trajectories avoid the obstacles. When the search reaches the vicinity of the target point and meets the preset tolerance conditions, the search process of the hybrid A* algorithm is terminated, and a feasible trajectory from the starting point to the target point is generated by backtracking the search path, which serves as the initial trajectory.

6. The method according to claim 5, characterized in that, The method further includes performing the following operations during the search process when obstacles are detected to be sparsely distributed or the current search node is close to the target point: Based on the kinematic model of the vehicle waiting to park and the attitude information of the target point, a geometric curve that satisfies the nonholonomic constraint is constructed, including a Dubins curve or a Reeds-Shepp curve. Calculate the corresponding geometric connection trajectory segment with the current search node as the starting point and the target point as the ending point; Feasibility testing is performed on the geometric connection trajectory segment; If all feasibility tests are passed, the geometric curve connection is confirmed to be successful. The current search node and the target point are directly connected through the geometric curve, a complete path is generated by backtracking, and the entire trajectory search process is terminated. If the feasibility test fails, the geometric connection attempt is abandoned, and the search continues using the conventional expansion strategy of the hybrid A* algorithm.

7. The method according to claim 1, characterized in that, The optimization of the generated initial trajectory based on the objective function and constraints to adjust the initial trajectory into a smooth trajectory includes: Using the trajectory nodes of the initial trajectory as optimization variables, a trajectory optimization model is constructed. The optimization model includes the objective function and the constraints. The objective function includes a smoothness objective function, a safety objective function, and a length and efficiency objective function. The constraints include kinematic and dynamic constraints of the vehicle waiting to park. The optimization model is solved using an operator splitting quadratic programming algorithm to adjust the initial trajectory into a smooth trajectory that meets the constraints.

8. A parking trajectory planning system based on heuristic search, characterized in that, The system includes: An obstacle detection module is used to detect obstacles around a vehicle waiting to park and to form a grid map based on the detected obstacles. The grid map includes drivable grids, obstacle grids, and unknown grids. A trajectory planner receives the grid map transmitted by the obstacle detection module, and the trajectory planner is configured to execute the parking trajectory planning method based on heuristic search according to any one of claims 1 to 7.

9. An electronic device, characterized in that, The electronic device includes a memory and a processor, the memory including instructions, and the processor executing the instructions to implement the parking trajectory planning method based on heuristic search according to any one of claims 1 to 7.

10. A program product, characterized in that, The program product includes instructions that, when executed, implement the parking trajectory planning method based on heuristic search according to any one of claims 1 to 7.

Images