Search type and geometric type path-based parking in-garage route construction method and device

Abstract

The invention discloses a parking garage entering route construction method and device based on search type and geometric type paths. The parking garage entering route construction method comprises the steps that whether a vehicle receives an effective parking instruction or not and enters a parking range and parking space information or not is judged, and effectiveness verification is carried out; path point sampling is carried out, obstacles are judged, and if the path is not feasible, the process is terminated; candidate paths are generated according to the sampling points and a vehicle and bicycle kinematics model, collision detection is carried out, and effective paths are screened; evaluating the path, selecting an optimal path, splicing the optimal path with the search path, and outputting a final parking path for the vehicle to execute; according to the parking garage entering route construction method, a search type route planning method and a geometric type route planning method are combined, through combination of a vehicle and bicycle kinematics model and a Reed-Shepp geometric route model, splicing optimization of a geometric route and a search route is further carried out, computing resources and consumed time are remarkably reduced, and the parking garage entering route construction method is suitable for large-scale popularization and application. The method is more suitable for an embedded platform chip to improve the calculation performance.

Description

Technical Field

[0001] This invention relates to the field of parking information processing technology, and in particular to a method and apparatus for constructing parking entry routes based on search-based and geometric paths. Background Technology

[0002] With the continuous development of autonomous driving technology, automated parking systems have become an important application area for intelligent vehicles. The goal of automated parking systems is to achieve fast, accurate, and safe parking operations in complex environments, such as narrow parking spaces and areas with dense obstacles. To meet the requirements of automated parking, path planning, as one of the core technologies of the system, is crucial. Currently, path planning methods in automated parking systems are mainly divided into two categories: geometric path planning and search path planning. Geometric path planning can quickly generate paths, facilitating tracking and execution. However, its disadvantage lies in the poor reliability of path execution; once the generated path is determined, it is difficult to quickly adjust online to adapt to suddenly appearing pedestrians or vehicles. Especially in scenarios with high precision requirements, it cannot effectively adapt to the dynamic constraints of the vehicle and changes in the environment.

[0003] Search-based path planning methods generate accurate paths by comprehensively searching the path space. They are highly adaptable to complex environments and conform to vehicle kinematics. However, they are computationally intensive and time-consuming, especially in complex scenarios where path searching requires significant computation time, leading to path generation delays (often reaching seconds or even tens of seconds). This makes it difficult to meet the real-time and rapid response requirements of automated parking systems, resulting in poor real-time performance.

[0004] As autonomous driving advances into more complex scenarios, single planning methods are no longer sufficient to meet practical needs. Especially on resource-constrained in-vehicle embedded platforms, generating a safe, smooth, and efficient parking trajectory within a limited computing time has become a key technological bottleneck restricting the large-scale mass production and application of automated parking functions. Summary of the Invention

[0005] To address the technical problems in the prior art, one objective of this invention is to provide a method and apparatus for constructing parking routes based on search-based and geometric paths. Another objective of this invention is to provide an apparatus for constructing the aforementioned parking route construction method based on search-based and geometric paths.

[0006] One of the objectives of this invention is achieved through the following technical solution:

[0007] A parking route construction method based on search-based and geometric paths includes the following steps:

[0008] S1: Determine whether the vehicle has received a valid parking instruction and confirm whether the vehicle has entered the parking area. If no instruction has been received or the vehicle has not entered the parking area, the task will not be executed.

[0009] S2: Obtain the parking location information and perform validity verification. If the parking location information is abnormal, do not execute the parking entry.

[0010] S3: Based on the vehicle's sensors and garage map information, route points are sampled, and obstacle information in the garage is determined. If there is obstacle information that makes parking impossible, the parking process is terminated.

[0011] S4: Based on the sampling points, generate a geometric path bundle from the vehicle's current position to multiple candidate target points using the vehicle-bicycle kinematics model and the Reeds-Shepp geometric path model, and perform collision detection to filter out invalid paths that interfere with obstacles, map boundaries, or garage lines;

[0012] S5: Based on the gear position, length, route end and current vehicle heading information of the geometric path, evaluate the generated path and calculate the evaluation function to determine the preferred geometric path;

[0013] S6: Based on the preferred geometric path, generate a search path for the vehicle to execute, and concatenate the search path with the preferred geometric path to output the optimal parking path.

[0014] Furthermore, in step S6, a hybrid A* algorithm is used to search for the endpoint of the path, and then the generated geometric path is added to the searched path.

[0015] Specifically, the vehicle-bicycle kinematics model used in step S4 has a state space that includes the vehicle's position coordinates, heading angle, and front wheel steering state in the global coordinate system. Based on the Reeds-Shepp model, the forward and backward movements of left turns, right turns, and straight-line driving are combined and modeled. By parametrically calculating the spatial position and heading changes of each motion primitive, a geometric parking path that meets the vehicle steering curvature constraint is constructed.

[0016] Furthermore, the motion primitives include the following primitive combinations: , , , , , , , , In this symbol, L represents a left-turn arc, R represents a right-turn arc, S represents a straight line segment, the superscript "+" indicates forward movement, and the superscript "-" indicates backward movement.

[0017] The algorithm for generating a single geometric path in step S4 includes the following steps:

[0018] Enumerate all candidate paths connecting the starting point and the target point, which are composed of the motion primitives;

[0019] During the enumeration process, if the length of a candidate path exceeds a threshold set based on the distance between the starting point and the target point, the further construction of that path will be terminated.

[0020] For each candidate path that has been constructed, calculate its total length and the minimum turning radius or maximum turning curvature on the path;

[0021] From the completed candidate path set, the optimal path is selected based on the total path length and the maximum absolute curvature of the path.

[0022] Furthermore, the threshold set based on the distance between the starting point and the target point is more than twice the straight-line distance between the starting point and the target point.

[0023] Furthermore, in step S4, the following optimization process is performed on the generated candidate geometric path:

[0024] End-point attitude alignment: For a path whose target is a perpendicular parking space, a constraint is applied to the end of the path to ensure that its deviation from the target parking direction is less than a predetermined angle threshold.

[0025] Safety buffer: Offset a point on the path by a preset safety distance in a direction perpendicular to the tangent of the path at that point, so as to establish a safety margin between the path and obstacles.

[0026] Furthermore, the path evaluation function used in step S5 is a weighted combination of multiple path evaluation indicators, including path length cost, shift number cost, end heading angle error cost, path curvature smoothness cost, and obstacle safety cost.

[0027] Optionally, in step S4, obstacle interaction and safety determination are performed through the following steps:

[0028] Vehicle body envelope space mapping: For each pose state on the path, the vehicle's outline is mapped from the vehicle coordinate system to the global coordinate system through coordinate transformation to obtain the actual space occupied by the vehicle;

[0029] Obstacle distance field construction: Based on environmental perception data, the directed distance from any point in the global coordinate system to the nearest obstacle surface is calculated to form a distance field;

[0030] Collision risk assessment: The collision risk of the path is assessed by determining the geometric relationship between the space occupied by the vehicle body and the obstacles, or by querying the distance field to obtain the safe distance at the path point.

[0031] Optionally, the path concatenation in S6 is achieved through the following threshold constraint:

[0032] Position continuity constraint: The Euclidean distance between the endpoints of the geometric path and the search path is less than a first position threshold;

[0033] Heading angle continuity constraint: The difference in heading angle between the geometric path and the search path is less than a first angle threshold;

[0034] Curvature continuity constraint: The difference in curvature between the geometric path and the search path is less than the first curvature threshold.

[0035] Further, the first position threshold is 10cm; the first angle threshold is 3°; the first curvature threshold is... .

[0036] The second objective of this invention is achieved through the following technical solution:

[0037] A parking route construction device based on search-based and geometric path generation; the device includes:

[0038] The parking instruction receiving module is used to receive and determine whether the vehicle has received a valid parking instruction, and to confirm whether the vehicle has entered the parking area.

[0039] The storage location information acquisition and verification module is used to acquire parking entry storage location information and perform validity verification.

[0040] The path sampling and obstacle detection module is used to sample path points based on vehicle sensors and garage map information, and to determine whether there are obstacles on the path that make parking impossible.

[0041] The geometric path generation and collision detection module is used to generate candidate paths based on the vehicle and bicycle kinematics model and the Reeds-Shepp geometric path model, and to perform collision detection to filter invalid paths;

[0042] The path evaluation and optimization module is used to calculate the path evaluation function based on factors such as path length, number of gear shifts, terminal heading angle error, path smoothness, and obstacle safety cost, and select the optimal path.

[0043] The path splicing and execution module is used to splice the generated geometric path with the search path and output the optimal parking path for the vehicle to execute.

[0044] Compared with the prior art, the present invention has at least the following beneficial effects:

[0045] 1. The parking route construction method of this invention combines search-based and geometric path planning methods. By combining a vehicle-bicycle kinematic model and a Reeds-Shepp geometric path model, it predefines the optimal path element combination by enumerating preset finite motion primitives such as left turn, right turn, and forward / backward movement in straight lines. This allows for the rapid generation of multiple candidate paths, and collision detection is used to initially filter valid paths. Furthermore, when generating a single path, a length truncation condition is introduced, setting a truncation threshold of more than twice the straight-line distance between the starting point and the target point to remove invalid branches. Therefore, this greatly simplifies the path construction calculation process, reduces the time delay of path planning, and thus significantly improves the response speed of parking tasks.

[0046] 2. Furthermore, by utilizing collision detection and obstacle distance field construction, paths interfering with obstacles are filtered out in real time during path construction, avoiding the enormous computational waste caused by generating all paths first and then performing global collision detection. In addition, further optimization processes (such as end-point attitude alignment and safety buffering) can effectively prevent collisions or accidental entry into obstacle areas due to unsuitable paths, further reducing the generation of invalid driving routes. This enables rapid elimination of invalid routes in complex environments, improving the reliability and safety of the automatic parking system.

[0047] 3. This invention optimizes the splicing of the geometric path and the search path, and through strict control of positional continuity, heading angle continuity, and curvature continuity, the generated path not only satisfies basic geometric constraints but also considers vehicle motion performance and operational efficiency. Specifically, the comprehensive evaluation of gear shift count, end-point heading angle error, path curvature smoothness, and obstacle safety distance during path generation effectively reduces abrupt steering and uneven driving during vehicle execution, ensuring the smoothness and stability of path execution. This makes the generated parking path more suitable for the smooth execution of autonomous vehicles, reducing complex operations and unnecessary adjustments during parking, and improving the smoothness and safety of the parking process.

[0048] 4. This invention, through the Reeds-Shepp model and a finite combination of motion primitives, can efficiently generate a limited set of path candidates, avoiding unnecessary forward and backward movements. Furthermore, the optimization strategy in the path evaluation function effectively reduces the generation of invalid turns, decreasing unnecessary computation and path adjustments, thereby saving computational resources. By reducing invalid computation and employing efficient algorithms, the use of hardware resources is optimized, reducing processor load and power consumption, significantly reducing computational resources and time consumption, making it more suitable for improving the computational performance of embedded platform chips. Attached Figure Description

[0049] Figure 1This is a flowchart illustrating the parking route construction method based on search-based and geometric paths according to Embodiment 1 of the present invention.

[0050] Figure 2 This is a schematic diagram of the parking route construction device based on search-based and geometric paths according to Embodiment 2 of the present invention.

[0051] Figure 3 The algorithm implementation effect diagram of the parking route construction method based on search-based and geometric paths in Embodiment 1 of the present invention is shown in the figure.

[0052] In the diagram: 10. Parking instruction receiving module; 20. Parking space information acquisition and verification module; 30. Path sampling and obstacle detection module; 40. Geometric path generation and collision detection module; 50. Path evaluation and optimization module; 60. Path splicing and execution module. Detailed Implementation

[0053] To facilitate understanding of the present invention, the technical solutions and advantages of the invention will be further described in detail below with reference to the accompanying drawings and embodiments. Any mechanisms or methods not elaborated in this invention can be referred to in the prior art. The specific structures and features of the present invention are illustrated below by way of example and should not be construed as limiting the present invention in any way. Furthermore, any of the technical features mentioned below (including implicit or disclosed features), as well as any technical features directly shown or implied in the figures, can be arbitrarily combined or deleted among these technical features to form more other embodiments that may not be directly or indirectly mentioned in this invention. The accompanying drawings show preferred embodiments of the present invention. However, the present invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the disclosure of the present invention.

[0054] Example 1

[0055] like Figure 1 , Figure 3 As shown in the figure, this application provides a parking route construction method and apparatus based on search-based and geometric paths, which can be applied in unstructured open space scenarios, greatly improving parking efficiency and providing effective support for the mass production of autonomous vehicles and the deployment of algorithms on embedded chip platforms.

[0056] To better understand the embodiments of this application, the embodiments of this application will be described in detail below.

[0057] This application provides a parking route construction method based on search-based and geometric paths, which includes the following steps:

[0058] S1: Determine whether the vehicle has received a valid parking instruction and confirm whether the vehicle has entered the parking area. If no instruction has been received or the vehicle has not entered the parking area, the task will not be executed.

[0059] In this embodiment, the surrounding environment is detected by onboard sensors (such as radar, cameras or lidar) to determine whether there is a parking target (such as parking lines, parking space signs, etc.), and the parking instruction is determined in combination with the vehicle's status.

[0060] This ensures accurate triggering of parking tasks; the system only initiates parking operations when a valid parking instruction is received and the vehicle enters the parking area, avoiding invalid operations and improving safety and efficiency. Furthermore, by determining the vehicle's position, the system ensures that the parking task is initiated at the appropriate time, avoiding erroneous triggering or meaningless operations.

[0061] S2: Obtain the parking location information and perform validity verification. If the parking location information is abnormal, do not execute the parking entry.

[0062] In this embodiment, onboard sensors (such as cameras and LiDAR) can acquire real-time parking space information within the garage. Combined with a pre-loaded garage map, this information is used to determine if a parking space is occupied. Furthermore, in-vehicle or external cameras are used to monitor the parking situation in real-time, determining if there are obstacles or other vehicles in the parking spaces. Additionally, it verifies whether the target parking space is large enough to accommodate the current vehicle.

[0063] Once the parking space information is confirmed to be valid and available in step S2, the system will continue to execute step S3.

[0064] S3: Based on the vehicle's sensors and garage map information, route points are sampled, and obstacle information in the garage is determined. If there is obstacle information that makes parking impossible, the parking process is terminated.

[0065] During this process, waypoints are sampled based on onboard sensors and garage map information to detect obstacles in the garage in real time. By comparing the space surrounding the vehicle with the obstacles, the system determines whether the path is blocked. If any obstacle (such as other vehicles or fixed obstacles) is detected that would impede normal parking and render the parking space unusable, the system will filter out the path and terminate the parking process.

[0066] S4: Based on the sampling points, generate a geometric path bundle from the vehicle's current position to multiple candidate target points using the vehicle-bicycle kinematics model and the Reeds-Shepp geometric path model, and perform collision detection to filter out invalid paths that interfere with obstacles, map boundaries, or garage lines;

[0067] In this embodiment, the Bicycle Kinematic Model used in step S4 serves as the theoretical basis for path generation and verification. By representing the vehicle as equivalent to front and rear virtual wheels, it describes the geometric constraints between the vehicle's position, heading, and steering under low-speed driving conditions. This provides a unified kinematic basis for the generation, evaluation, collision detection, and feasibility of parking paths, ensuring that the generated parking paths meet the vehicle's minimum turning radius and steering continuity requirements, thereby guaranteeing the path's executability on actual vehicles. Its state space includes the vehicle's position coordinates, heading angle, and front wheel steering state in the global coordinate system.

[0068] For example, the vehicle-bicycle kinematic model described in S4 is described using the following five-dimensional state vector:

[0069]

[0070] in, These are the two-dimensional position coordinates of the vehicle in the global coordinate system, representing the horizontal and vertical coordinates of the vehicle's position. The yaw angle is the angle between the vehicle's longitudinal axis and the global coordinate system's X-axis. This refers to the vehicle's linear velocity, which is the speed at which the vehicle travels along its current heading. This is the steering angle of the vehicle's front wheels, which can be used to construct the front wheel steering state.

[0071] In this way, the model ensures that all generated paths conform to the nonholonomic constraints and kinematic properties of the vehicle, which facilitates subsequent path calculations.

[0072] Furthermore, based on the Reeds-Shepp model, the forward and backward movements of left turns, right turns, and straight-line driving are modeled in combination. By parametrically calculating the spatial position and heading changes of each motion primitive, a geometric parking path that satisfies the vehicle's steering curvature constraints is constructed. In this embodiment, the primitives such as the forward and backward movements of left turns, right turns, and straight-line driving are the basic units constituting the geometric path. Through their combination, a set of candidate paths that connect any two poses and meet the vehicle's minimum turning radius constraints can be efficiently constructed. This avoids blind searching in continuous space and greatly improves the initial path generation speed.

[0073] Further refining, the Reeds-Shepp primitives are defined as nine different path patterns, including the following primitive combinations: , , , , , , , , In this symbol, L represents a left-turn arc, R represents a right-turn arc, S represents a straight line segment, the superscript "+" indicates forward movement, and the superscript "-" indicates backward movement.

[0074] For example, the motion primitives are defined as follows:

[0075]

[0076] in, and They are left turn and right turn, respectively. The superscript "+" indicates forward movement, and "-" indicates backward movement. That is, based on the vehicle's current state information, appropriate motion primitives are selected, and corresponding path segments are generated. Each path segment contains at least one left turn, one right turn, and one straight line segment.

[0077] More preferably, in the process of generating path segments in step S4 above, the algorithm for generating a single geometric path includes the following steps:

[0078] 1) Enumerate all connection points formed by the combination of the aforementioned motion primitives. With the target point Candidate paths include:

[0079] According to the formula Enumerate all feasible paths (combinations of motion primitives).

[0080] During the enumeration process, if the length of the candidate path exceeds the length based on the starting point... With the target point If a threshold value is set for the location distance, further construction of the path will be terminated; specifically, the path based on the starting point... With the target point The threshold for location distance is set to be more than twice the straight-line distance between the starting point and the target point. If the value exceeds this limit, the construction of the current path will be terminated. This design can remove invalid branches in time, avoid generating excessively long invalid paths, and reduce the amount of computation.

[0081] In this embodiment, the starting point Typically, it indicates the vehicle's current location and status, which can be determined using location coordinates. vehicle heading angle ,speed and turning Information such as these is obtained through real-time calculations. And the target point... This refers to the specific parking space or garage location where the vehicle is desired to be parked. This target point can be dynamically obtained based on parking task instructions, system settings, positioning systems, etc., which will not be elaborated here.

[0082] To further explain, in this embodiment, the nine motion primitives are the constituent elements for generating geometric paths, while The target distance of the path is determined, i.e., the objective of path generation. The path is optimized during the generation process to ensure that the distance from the starting point is within the target range. To the target point The path length and curvature are limited to a reasonable range.

[0083] For example, to achieve accurate path construction and collision detection by the computer, geometric parameterization calculations are required for each motion primitive. The core of this process is to discretize the path into a series of continuous state points (position and heading). The path generation algorithm is as follows:

[0084] a. Arc segment (with (For example)

[0085] For the circular arc segment in the vehicle path, assume the starting point of the arc is... The initial heading angle is curvature Given that the radius of the arc is... The parametric formula for the points on the path on the arc segment is:

[0086] a1. Position parameterization:

[0087]

[0088]

[0089] in, For maximum curvature, For path parameters, , This is the total rotation angle of the arc.

[0090] a2. Heading angle parameterization:

[0091]

[0092] in, For curvature, The path parameter for the arc length along the arc represents the change in the direction of the arc segment over time.

[0093] b. Straight line segment (with (For example)

[0094] For the straight segments in the vehicle path, assume the starting point of the straight segment is... The initial heading angle is and path length The parametric formula for a line segment is given below:

[0095] b1. Position parameterization:

[0096]

[0097]

[0098] in, For path parameters, , Let be the length of the line segment.

[0099] b2. Heading angle parameterization:

[0100] in, This is a fixed heading angle for a straight segment.

[0101] 2) Next, for each candidate path that has been constructed, calculate its total length and the minimum turning radius or maximum turning curvature on the path;

[0102] Assuming the path is... Segmented arc and The path is composed of straight line segments, and the length of each segment is calculated parametrically from the arc segments and straight line segments respectively. Total length It can be represented as:

[0103] in, For the first The angle change of the segmented arc; For the first The curvature of a segment of circular arc; For the first

[0104] The length of a straight line segment.

[0105] 3) Select the optimal path from the completed candidate path set based on the total path length and the maximum absolute curvature of the path.

[0106] This embodiment transforms the abstract path enumeration into concrete, computable geometric path bundles. It ensures that each generated path strictly adheres to vehicle kinematic constraints and lays a solid foundation for subsequent optimization decisions through quantitative calculations. It is a key module in the entire geometry-search hybrid programming method that guarantees planning quality and efficiency.

[0107] To further improve the safety, availability, and control accuracy of the planned path, optionally, in step S4, the generated candidate geometric path is further optimized as follows:

[0108] 1) End-point attitude alignment: For paths with a target perpendicular parking space, constraints are applied to the end of the path to ensure that the deviation from the target parking direction is less than a predetermined angle threshold; this ensures that the vehicle is basically parallel to the parking space direction after it stops, providing a basis for subsequent precise fine-tuning or direct termination of the planning.

[0109] In this embodiment, the orientation angle of the target parking space is defined in the global coordinate system as... For each candidate geometric path, calculate the heading angle of the vehicle at its end. ;

[0110] In this embodiment, the end-point attitude alignment specifically refers to adding hard constraints during perpendicular parking to adjust the vehicle's end-point attitude. and vertical direction Alignment, with the following constraints: ;in, The direction at the end of the vehicle parking path.

[0111] In this embodiment, the end-point orientation alignment constraint ensures that the vehicle can accurately align with the parking space when parking perpendicularly. This optimization ensures that the path selected from the geometric path bundle has an end-point orientation that closely matches the actual parking requirements, significantly reducing the difficulty and failure probability of subsequent search-based path planning adjustments.

[0112] 2) Safety buffer: Offset a point on the path by a preset safety distance in a direction perpendicular to the tangent of the path at that point, so as to establish a safety margin between the path and the obstacle.

[0113] In this embodiment, for each path point The path offset is performed using a safety buffer, and the offset formula is as follows: ;in =0.2m is the safety margin. path point The direction the vehicles are facing.

[0114] in, This means shifting vertically to the left in the direction of vehicle travel. During implementation, the shift can be made to the left or right depending on the actual distribution of obstacles, as long as the shift direction is away from the obstacles.

[0115] In this embodiment, the waypoint safety buffer ensures that the vehicle maintains a safe distance of at least 0.2 meters from obstacles in the parking path, thus preventing collisions. Furthermore, a safety margin is proactively introduced between the vehicle and obstacles during the planning phase, virtually inflating the vehicle's outline. By accurately determining the distance, the system proactively avoids potential collision risks caused by various errors during the planning stage, significantly enhancing the robustness and safety of the parking system.

[0116] Through the above-mentioned safety buffering process, a static safety boundary is established for the planned path.

[0117] To further combine external safety boundaries with dynamic environmental information to facilitate the calculation of the relative position between the vehicle and obstacles, and thus determine whether a collision has occurred, further, in step S4, obstacle interaction and safety determination are performed through the following steps:

[0118] 1) Vehicle body envelope space mapping: For each pose state on the path, the vehicle's outline is mapped from the vehicle coordinate system to the global coordinate system through coordinate transformation to obtain the actual space occupied by the vehicle; that is, the vehicle body envelope (rectangle). The envelope represents the vehicle's outer boundary. By transforming this boundary, the relationship between the vehicle and obstacles in path planning can be determined.

[0119] Specifically, the formula for transforming the vehicle body envelope is: ;in, This represents the transformed coordinates of the vehicle's envelope. In other words, it represents the position of the transformed vehicle's shape in the world coordinate system. It indicates the distance from the front axle to the vehicle's center of gravity (front overhang length). This indicates the distance from the rear axle to the vehicle's center of gravity (rear overhang length). Represents the width of the vehicle. Transformation matrix. Used to transform the vehicle's local coordinate system (vehicle body coordinate system) into the global coordinate system. The ± sign represents the different vertices of the envelope (the offset of the four vertices: front, back, left, and right).

[0120] In this embodiment, the vehicle's envelope position is accurately represented in the global coordinate system using a transformation matrix, thereby providing accurate geometric information for subsequent collision detection and preventing collisions during vehicle movement. This ensures that the path planning system can calculate and correct paths in real time under dynamic or complex environments.

[0121] 2) Obstacle Distance Field Construction: Based on environmental perception data, the directed distance from any point in the global coordinate system to the nearest obstacle surface is calculated to form a distance field; this supports efficient safety margin assessment and avoids time-consuming pairwise geometric calculations for each path point and each obstacle. In this embodiment, the obstacle distance field is constructed using the following formula: ;in, Indicates the current position of the vehicle; Indicates the first The center coordinates of the obstacles; Represents the radius of the i-th obstacle; Representing path points To the obstacle The distance, and Representing path points Safe distance to the obstacle (positive value indicates safety, negative value indicates intrusion).

[0122] 3) Collision risk assessment: The collision risk of the path is assessed by judging the geometric relationship between the space occupied by the vehicle body and the obstacle, or by querying the distance field to obtain the safe distance at the path point.

[0123] In this embodiment, by utilizing the obstacle distance field, the system can monitor the relationship between the vehicle's path and its surrounding environment in real time. When a point on the path is too close to an obstacle, the system adjusts the path according to the distance field to avoid collision risks. It is understood that when a point on the path is determined to be too close to an obstacle (i.e., there is a collision risk), the path will be directly eliminated in step S4 and will not proceed to the subsequent optimization process.

[0124] S5: Based on the gear position, length, route end and current vehicle heading information of the geometric path after the above collision detection screening, evaluate the generated path and calculate the evaluation function to determine the preferred geometric path;

[0125] For example, the path evaluation function used in step S5 is a weighted combination of multiple path evaluation indicators, including path length cost, shift number cost, end heading angle error cost, path curvature smoothness cost, and obstacle safety cost.

[0126] To further refine, optionally, the path evaluation function calculation formula in step S5 is as follows:

[0127]

[0128] in, These are the weighting coefficients; These include: path length cost Cost of shifting gears Terminal heading angle error cost Path curvature smoothness cost Obstacle safety costs .

[0129] Furthermore, the aforementioned indicators are defined as follows: ,in For path length, The maximum path length is used to select a shorter path and improve parking efficiency. ,in, The number of gears is assumed to be 3; the forward and reverse movements are measured to reduce unnecessary driving movements, improve driving smoothness, and reduce wear on the transmission system. ,in, For the desired heading angle, The current heading angle is used to ensure that the attitude at the end of the path is consistent with the target parking direction, creating favorable conditions for subsequent accurate parking. ,in, For path curvature, For maximum curvature, The integral of the square of the curvature along the path is used to measure the overall curvature of the path in order to select a smoother path that conforms to vehicle kinematic constraints and is easy to implement in practice. ,in, For the first on the path One sampling point; Representing path points Distance to the obstacle; For safety sensitivity coefficient, The total number of path sampling points controls the degree of influence of safety on the overall evaluation function. This evaluation function measures the safe distance between the path and obstacles. For each path point, if the path point is closer to an obstacle, its index value is smaller, thus increasing the safety margin. The value of represents the distance between the path point and the obstacle. The larger the value, the safer the path; it causes the path to rapidly increase its cost when approaching obstacles, thereby guiding the path to actively maintain a safe distance.

[0130] Therefore, by comprehensively considering path length, number of gear shifts, heading error, path smoothness, and safety, the system ensures that the path is not only the shortest but also executed smoothly and safely. This enables the autonomous driving system to comprehensively evaluate and select the optimal path, balancing different constraints (such as time, gear shifts, comfort, and safety), thereby improving the efficiency and reliability of the autonomous driving system.

[0131] In this embodiment, an evaluation function is designed to evaluate the generated geometric path by weighting gear position, length, and heading. The purpose is to further utilize the hybrid Astar algorithm to search for the latest path endpoint of the generated optimal path with lower weights, then add the generated geometry to the searched path, and output the optimal parking path, as follows:

[0132] S6: Based on the preferred geometric path, generate a search path for the vehicle to execute, and concatenate the search path with the preferred geometric path to output the optimal parking path.

[0133] Based on the optimized geometric path obtained in S5, and using its terminal state (position and heading) as initial conditions, a fine trajectory from that point to the final target point within the parking space is generated using a hybrid A* search algorithm; this is the search-based path. To ensure that the complete path formed after combining the two is smooth and feasible for the vehicle control system, strict continuity constraints must be met.

[0134] In this embodiment, the goal of step S6 is to connect the search path and the geometric path into a continuous, executable optimal parking path. During this process, it is necessary to ensure that the connection points (qc) meet the set constraints to guarantee smooth vehicle movement and path feasibility.

[0135] Optionally, the path concatenation in S6 is achieved through the following threshold constraint:

[0136] 1) Position continuity constraint: The Euclidean distance between the endpoints of the geometric path and the search path is less than a first position threshold;

[0137] Specifically, to ensure that the spliced ​​path has no abrupt changes at the connection point, the positional continuity constraint must be met; that is, the Euclidean distance between the endpoints of the two paths should be less than a preset threshold, as shown below:

[0138] ;in, This indicates the Reeds-Shepp path in the parameter. End point coordinates ; The starting coordinates of the search path Furthermore, the first position threshold is 10cm.

[0139] With this design, the Euclidean distance between the connection points is less than 10cm (about the width of a tire), ensuring that the vehicle will not experience obvious jumps or abrupt changes when splicing the path.

[0140] 2) Heading Angle Continuity Constraint: The difference in heading angle between the geometric path and the search path is less than a first angle threshold; in this embodiment, the first angle threshold is 3°. In this embodiment, to ensure that the vehicle's heading angle does not change abruptly at the path junction, it is necessary to ensure that the difference in heading angle between the two paths does not exceed a certain threshold. Excessive heading angle difference may cause the vehicle's steering wheel to turn sharply, affecting the smoothness of the parking process. Therefore, a constraint is imposed on the heading angle, referring to the following formula: ;in, The heading angle of the Reeds-Shepp route at its endpoint; The heading angle is the starting point of the search path. In this embodiment, the difference in heading angle between the two path endpoints is limited to less than 3° to avoid abrupt steering wheel corrections and ensure vehicle stability.

[0141] 3) Curvature continuity constraint: The curvature difference between the geometric path and the search path is less than a first curvature threshold. In this embodiment, the first curvature threshold is 0.1m. −1 The formula for curvature continuity constraint is as follows: ;in, The curvature of the Reeds-Shepp path at its endpoint; The curvature of the search path at the starting point is determined. This ensures that the curvature difference at the path junctions is less than 0.1m⁻¹, thereby avoiding drastic changes in the steering wheel angle (assuming a wheelbase of L=2.7).

[0142] First, the positional deviation between the end point of the geometric path and the starting point of the search path must be less than a first position threshold (e.g., 0.1 meters) to avoid vehicle position jumps. Second, the heading angle deviation between the two must be less than a first angle threshold (e.g., 3°) to prevent the steering wheel from turning sharply at the connection point. Finally, the path curvature deviation at the connection point between the geometric path and the search path must be less than a first curvature threshold (e.g., 0.1 m⁻¹) to ensure the continuity of front wheel steering angle changes and avoid sudden changes in lateral acceleration.

[0143] Thus, through a funnel-shaped hierarchical planning architecture of scene filtering → geometric funnel → search refinement, the system ultimately outputs a complete and smooth optimal parking path from the starting point to the parking space. This design combines efficient global geometric planning with precise local search planning, significantly improving path quality, planning success rate, and final execution smoothness while ensuring absolute safety and kinematic feasibility.

[0144] like Figure 3 As shown, this is a schematic diagram of parking paths in an unstructured drivable space. The diagram shows a parking area where the vehicle's path consists of two paths, representing a hybrid search path and a geometric path, respectively.

[0145] Hybrid Search Path (Blue Arrow): The blue path represents the vehicle's path selection from the starting point to the target location. It integrates search-based and geometric path planning technologies to achieve efficient path selection and avoid complex turns or invalid driving paths.

[0146] Geometric Path (Orange Arrow): The orange path is the path generated by the geometric path model, which relies on the vehicle and bicycle kinematics model for specific path planning.

[0147] This invention optimizes the splicing of geometric paths and search paths, and through strict control of positional continuity, heading angle continuity, and curvature continuity, generates paths that not only meet basic geometric constraints but also consider vehicle motion performance and operational efficiency. Specifically, the comprehensive evaluation of gear shift counts, end-point heading angle error, path curvature smoothness, and obstacle safety distances during path generation effectively reduces abrupt steering and uneven driving during vehicle execution, ensuring smooth and stable path execution. This makes the generated parking paths more suitable for the smooth execution of autonomous vehicles, reducing complex operations and unnecessary adjustments during parking, and improving the smoothness and safety of the parking process.

[0148] Example 2

[0149] like Figure 2 As shown, this application also provides a parking route construction device based on search-based and geometric paths; it should be understood that the construction device according to this embodiment can implement the following... Figure 1 The parking route construction method based on search and geometric paths shown above is described in detail above and will not be repeated here for the sake of brevity.

[0150] The construction apparatus includes:

[0151] The parking instruction receiving module 10 is used to receive and determine whether the vehicle has received a valid parking instruction, and to confirm whether the vehicle has entered the parking area.

[0152] The storage location information acquisition and verification module 20 is used to acquire parking entry storage location information and perform validity verification.

[0153] The path sampling and obstacle detection module 30 is used to sample path points based on vehicle sensors and garage map information, and to determine whether there are obstacles on the path that make parking impossible.

[0154] The geometric path generation and collision detection module 40 is used to generate candidate paths based on the vehicle and bicycle kinematics model and the Reeds-Shepp geometric path model, and to perform collision detection to filter invalid paths.

[0155] The path evaluation and optimization module 50 is used to calculate the path evaluation function based on factors such as path length, number of gear shifts, terminal heading angle error, path smoothness, and obstacle safety cost, and to select the optimal path.

[0156] The path splicing and execution module 60 is used to splice the generated geometric path with the search path and output the optimal parking path for the vehicle to execute.

[0157] The parking route construction device based on search-based and geometric paths provided in this embodiment 2 significantly improves the intelligence, reliability and efficiency of the automatic parking system through the cooperation of multiple modules.

[0158] First, the parking instruction receiving module and the parking space information acquisition and verification module ensure the validity of parking instructions and the availability of parking spaces, avoiding the execution of invalid instructions and unsuitable parking spaces, thus improving the accuracy and security of the system.

[0159] Secondly, the path sampling and obstacle detection module combines vehicle sensor and garage map information to sample the path in real time and detect obstacles, ensuring that the selected path is unobstructed and preventing collisions during parking. The geometric path generation and collision detection module, based on the vehicle-bicycle kinematics model and the Reeds-Shepp geometric path model, performs collision detection on candidate paths, further filtering out infeasible paths, thereby improving parking safety.

[0160] The path evaluation and optimization module comprehensively considers multiple factors, such as path length, number of gear shifts, end-point heading angle error, path smoothness, and obstacle safety costs, to optimize path selection and ensure the vehicle completes the parking task along the optimal path, thus improving path accuracy and stability. Finally, the path stitching and execution module uses smooth path stitching technology to ensure a smooth vehicle execution process, avoiding abrupt or uncoordinated path transitions and enhancing the smoothness of the automatic parking experience.

[0161] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above description is only a specific embodiment of this application and is not intended to limit the scope of protection of this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A method for constructing parking entry routes based on search-based and geometric paths, characterized in that, Includes the following steps: S1: Determine whether the vehicle has received a valid parking instruction and confirm whether the vehicle has entered the parking area. If no instruction has been received or the vehicle has not entered the parking area, the task will not be executed. S2: Obtain the parking location information and perform validity verification. If the parking location information is abnormal, do not execute the parking entry. S3: Based on the vehicle's sensors and garage map information, route points are sampled, and obstacle information in the garage is determined. If there is obstacle information that makes parking impossible, the parking process is terminated. S4: Based on the sampling points, generate a geometric path bundle from the vehicle's current position to multiple candidate target points using the vehicle-bicycle kinematics model and the Reeds-Shepp geometric path model, and perform collision detection to filter out invalid paths that interfere with obstacles, map boundaries, or garage lines; S5: Based on the gear position, length, route end and current vehicle heading information of the geometric path, evaluate the generated path and calculate the evaluation function to determine the preferred geometric path; S6: Based on the preferred geometric path, generate a search path for the vehicle to execute, and concatenate the search path with the preferred geometric path to output the optimal parking path.

2. The parking route construction method based on search-based and geometric paths as described in claim 1, characterized in that, The vehicle-bicycle kinematics model used in step S4 has a state space that includes the vehicle's position coordinates, heading angle, and front wheel steering state in the global coordinate system. Based on the Reeds-Shepp model, the forward and backward movements of left turns, right turns, and straight-line driving are combined and modeled. By parametrically calculating the spatial position and heading changes of each motion primitive, a geometric parking path that meets the vehicle steering curvature constraint is constructed.

3. The parking route construction method based on search-based and geometric paths as described in claim 2, characterized in that, The motion primitives include the following combinations of primitives: , , , , , , , , In this symbol, L represents a left-turn arc, R represents a right-turn arc, S represents a straight line segment, the superscript "+" indicates forward movement, and the superscript "-" indicates backward movement.

4. The parking route construction method based on search-based and geometric paths as described in claim 3, characterized in that, The step S4 of generating a single geometric path includes the following steps: Enumerate all candidate paths connecting the starting point and the target point, which are composed of the motion primitives; During the enumeration process, if the length of a candidate path exceeds a threshold set based on the distance between the starting point and the target point, the further construction of that path will be terminated. For each candidate path that has been constructed, calculate its total length and the minimum turning radius or maximum turning curvature on the path; From the completed candidate path set, the optimal path is selected based on the total path length and the maximum absolute curvature of the path.

5. The parking route construction method based on search-based and geometric paths as described in claim 4, characterized in that, In step S4, the following optimization process is further performed on the generated candidate geometric path: End-point attitude alignment: For a path whose target is a perpendicular parking space, a constraint is applied to the end of the path to ensure that its deviation from the target parking direction is less than a predetermined angle threshold. Safety buffer: Offset a point on the path by a preset safety distance in a direction perpendicular to the tangent of the path at that point, so as to establish a safety margin between the path and obstacles.

6. The parking route construction method based on search-based and geometric paths as described in claim 1, characterized in that, The path evaluation function used in step S5 is a weighted combination of multiple path evaluation indicators, including path length cost, shift number cost, end heading angle error cost, path curvature smoothness cost, and obstacle safety cost.

7. The parking route construction method based on search-based and geometric paths as described in claim 1, characterized in that, In step S4, obstacle interaction and safety determination are performed through the following steps: Vehicle body envelope space mapping: For each pose state on the path, the vehicle's outline is mapped from the vehicle coordinate system to the global coordinate system through coordinate transformation to obtain the actual space occupied by the vehicle; Obstacle distance field construction: Based on environmental perception data, the directed distance from any point in the global coordinate system to the nearest obstacle surface is calculated to form a distance field; Collision risk assessment: The collision risk of the path is assessed by determining the geometric relationship between the space occupied by the vehicle body and the obstacles, or by querying the distance field to obtain the safe distance at the path point.

8. The parking route construction method based on search-based and geometric paths as described in claim 1, characterized in that, The path concatenation in S6 is achieved through the following threshold constraints: Position continuity constraint: The Euclidean distance between the endpoints of the geometric path and the search path is less than a first position threshold; Heading angle continuity constraint: The difference in heading angle between the geometric path and the search path is less than a first angle threshold; Curvature continuity constraint: The difference in curvature between the geometric path and the search path is less than the first curvature threshold.

9. The parking route construction method based on search-based and geometric paths as described in claim 8, characterized in that, The first position threshold is 10cm; the first angle threshold is 3°; the first curvature threshold is 0.1m. −1 .

10. A parking route construction device based on search-based and geometric path generation, characterized in that, The construction apparatus includes: The parking instruction receiving module is used to receive and determine whether the vehicle has received a valid parking instruction, and to confirm whether the vehicle has entered the parking area. The storage location information acquisition and verification module is used to acquire parking entry storage location information and perform validity verification. The path sampling and obstacle detection module is used to sample path points based on vehicle sensors and garage map information, and to determine whether there are obstacles on the path that make parking impossible. The geometric path generation and collision detection module is used to generate candidate paths based on the vehicle and bicycle kinematics model and the Reeds-Shepp geometric path model, and to perform collision detection to filter invalid paths; The path evaluation and optimization module is used to calculate the path evaluation function based on factors such as path length, number of gear shifts, terminal heading angle error, path smoothness, and obstacle safety cost, and select the optimal path. The path splicing and execution module is used to splice the generated geometric path with the search path and output the optimal parking path for the vehicle to execute.

Images