Multi-step parallel parking track planning method based on key frame division

Abstract

The invention belongs to the technical field of intelligent vehicle automatic parking, and relates to a two-stage optimal parallel parking trajectory planning method based on key frame division. The objective of the invention is to solve the problems of limited parking space, high collision risk, slow optimization convergence and low terminal attitude precision in narrow parking spaces in cities. According to the technical scheme, the method is characterized in that a parking scene model and kinematics constraints are constructed, and according to key frames, the method is divided into a C-shaped garage entering stage (two sections of right-turn arcs, and tail garage entering is ensured) and an S-shaped garage rolling stage (a tubular space S-shaped structure, and reversible displacement attitude adjustment); building a multi-objective optimization model by stages, dispersing by a fourth-order Runge-Kutta method, and converting to nonlinear programming for solving; a local hot start strategy is introduced to improve the convergence speed. The method is mainly used for automatic parking of urban compact parking spaces, and safety, precision and real-time performance are improved.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle automatic parking technology, specifically involving a two-stage optimal parallel parking trajectory planning method based on keyframe partitioning. It is applicable to automatic parallel parking systems in urban road narrow parking space scenarios, providing core algorithm support for intelligent vehicles to achieve accurate and safe automatic parking. Background Technology

[0002] With the continuous growth of urban car ownership, parking space resources are becoming increasingly scarce, and the demand for parking in narrow parallel parking spaces (where the length of the space is close to the length of the vehicle and the width is only slightly larger than the vehicle body) is becoming increasingly prominent. Traditional parallel parking trajectory planning methods mostly adopt a single-stage planning strategy, that is, directly generating a complete trajectory from the initial position to the target parking space, which has the following shortcomings:

[0003] High collision risk: The vehicle's motion envelope is complex in a narrow space, and the single-stage trajectory is easy to coarsely depict the collision avoidance constraint. In particular, dynamic collisions between the front and rear corners of the vehicle and the boundary of the parking space are difficult to avoid accurately.

[0004] Slow convergence: The nonlinear optimization of the complete trajectory involves many variables and high dimensionality, resulting in a large number of solver iterations and poor real-time performance, making it difficult to meet the user experience requirement of "second-level response" for automatic parking;

[0005] Low terminal posture accuracy: Single-stage planning has insufficient control over the terminal posture (yaw angle, alignment of vehicle body and parking space) of "final parallel parking", which can easily lead to problems such as "vehicle body tilting into the parking space" and "not fully parked".

[0006] To address the aforementioned issues, some studies have attempted to introduce a "multi-stage planning" approach. However, existing multi-stage methods suffer from drawbacks such as ambiguous keyframe definitions, poor inter-stage connectivity, and a lack of warm-start strategies, and still cannot achieve the goal of "safe, accurate, and efficient" parking in narrow parking space scenarios.

[0007] This disclosure addresses, but is not limited to, the many factors mentioned above. Summary of the Invention

[0008] This invention belongs to the field of intelligent vehicle technology, specifically relating to a two-stage optimal parallel parking trajectory planning method based on keyframe partitioning. It aims to address a series of challenges faced by automated parking in urban scenarios with narrow parallel parking spaces, including high collision risk, slow optimization convergence, and low terminal attitude accuracy. Through a comprehensive technical solution of "staged precise control + keyframe state connection + local hot-start acceleration," this invention aims to provide an efficient, safe, and accurate core algorithm for automated parallel parking, offering solid technical support for the application of intelligent vehicles in complex parking environments.

[0009] To address the shortcomings of traditional automated parking technology in confined spaces, the core technical solution of this invention is a two-stage optimal parallel parking trajectory planning method based on keyframe partitioning. This method creatively decouples the entire complex parking process into two independently modelable and optimizable stages, and designs a series of innovative algorithms and strategies to ensure the physical feasibility, collision avoidance safety, and computational efficiency of the planned trajectory.

[0010] The steps used in this invention are as follows:

[0011] Defining the Parking Problem and Establishing a Model: This invention first defines the parking trajectory planning problem as an optimal control problem. Its core is to find a time-parameterized continuous trajectory in a parking scenario that satisfies initial and final boundary conditions, remains in free space, and is constrained by the vehicle dynamics model. Based on the Ackermann front-wheel steering principle, a vehicle kinematic model is established, describing the vehicle's motion state as a vector containing its lateral position, longitudinal position, and heading angle. The center of the vehicle's rear axle is selected as the state observation point.

[0012] Constraint system construction: To ensure the feasibility and safety of the trajectory, this invention constructs a strict constraint system.

[0013] 1. Vehicle kinematic constraints: Strict upper and lower limits are imposed on the vehicle's speed, acceleration, front wheel angle, and rate of change of angle to ensure that all motion parameters are within the limits of the vehicle's mechanical performance and to avoid uncomfortable or unsafe conditions such as rapid acceleration and sharp turns.

[0014] 2. Boundary Constraints for State and Control Variables: Precise constraints are applied to the initial and final states to ensure a smooth transition of the vehicle from a known initial pose to the target parking pose. The final constraint is particularly critical, as it forces the vehicle to be perfectly parallel to the parking line and precisely aligned at the end of parking, meeting the core requirements of "parallel parking and precise alignment," effectively solving the problem of low final pose accuracy in traditional methods.

[0015] 3. Collision Avoidance Constraints: This invention models both vehicles and obstacles as convex polygons and employs a convex constraint modeling method based on duality properties. By introducing auxiliary variables and a series of linearization operations, the traditional, non-differentiable constraint of "the minimum distance between the vehicle and the obstacle is greater than a safety threshold" is equivalently transformed into a set of differentiable convex constraints. This method significantly improves the differentiability and tractability of obstacle avoidance constraints in the optimal control solution process, enabling seamless integration of collision avoidance constraints with multi-objective optimization models, fundamentally ensuring the safety of the parking process and the accuracy of obstacle avoidance.

[0016] Parking Feasibility Analysis: To simplify the complex parking trajectory planning problem, this invention conducts an in-depth analysis of the vehicle's motion characteristics in a confined space. Based on two basic path structures: the S-shaped basic steering structure and the C-shaped path structure, it proves the feasibility of the vehicle making precise position and attitude adjustments in a narrow space.

[0017] Two-stage division: Based on the differences in path structure and control behavior during the parking process, the parking process is divided into two typical stages.

[0018] 1. First Stage (C-Shaped Parking): This stage's path consists of two right-turning arcs in the same direction, forming a unique "C"-shaped trajectory. This trajectory has a natural "outward expansion" tendency, effectively avoiding the risk of the vehicle's front (e.g., front corner) colliding with the parking space boundary or adjacent vehicles when reversing into the parking space, thus improving the safety and success rate of parking. The endpoint of this stage is defined as a keyframe, with the constraint that the rear of the vehicle is completely inside the parking space and has a good basic reversing posture.

[0019] 2. Second Stage (S-shaped Parking): This stage restricts the vehicle to reciprocating adjustments within a tubular space. A reversible displacement strategy based on an S-shaped steering structure is employed. Through multiple combinations of forward and backward movements, the vehicle's attitude and position are gradually corrected, ultimately achieving perfect parallel alignment between the vehicle and the parking line. This meticulous adjustment ensures high precision in the final parking position, solving the problem of achieving perfect parking using traditional methods.

[0020] Constructing a two-stage optimal control model: This invention designs corresponding multi-objective optimization functions for the two stages, taking into account their different characteristics.

[0021] 1. The objective function for the first stage includes a time minimization term, a smoothing term of the weighted sum of squares of the control inputs (covering acceleration, front wheel angular velocity, vehicle speed, and steering angle), and a term to minimize the final direction angle. The optimization objective of this stage is to safely enter the parking space in the shortest time and ensure good controllability of the vehicle's attitude when entering the second stage.

[0022] 2. The objective function for the second stage includes a time minimization term, a control input smoothing term, and a maneuver minimization term. Additionally, a penalty term for changes in speed direction is introduced to avoid frequent forward / reverse switching and improve the smoothness of the parking process.

[0023] Model Solution: The optimal control problem is discretized into a nonlinear programming problem using the fourth-order Runge-Kutta method, and then solved sequentially using an efficient solver. The endpoint state of the first stage is used as the initial condition for the second stage to ensure a smooth transition between the two stages, achieving continuity and coherence of the overall path.

[0024] A local hot start acceleration strategy is designed: In order to solve the problems of slow solution speed and poor real-time performance of nonlinear programming in the optimal control method, this invention proposes a planning acceleration strategy based on local hot start for the parking and huddling stage.

[0025] Kneading Trajectory Template Library: This strategy leverages the strong regularity of the trajectory structure during the kneading phase to pre-build a trajectory library containing various typical adjustment templates.

[0026] Optimization of initial solution generation: During actual parking, the system quickly matches the closest template trajectory in the template library based on the current parking space length and the initial state of the key frame, and uses it as the initial solution of the nonlinear optimizer.

[0027] This strategy avoids the optimizer performing a large-scale search from scratch, significantly improving the solver's convergence speed and stability. Simulation tests show that this strategy reduces the solution time for the library scrambling phase from the traditional 6.79 seconds to 4.68 seconds, greatly ensuring the system's real-time response capability and practicality.

[0028] This invention offers the following advantages: By employing C-shaped trajectory design and a convex constraint collision avoidance modeling method based on duality, it fundamentally reduces collision risks during parking, effectively avoiding front and rear corner collisions between the vehicle and obstacles, thus significantly improving parking safety. Simultaneously, the use of a local hot-start acceleration strategy drastically shortens the optimization solution time, ensuring the system completes trajectory planning within seconds. Combined with phased optimization and refined objective function design, the total parking time is effectively reduced. Furthermore, precise definition of keyframes and fine adjustment during the S-shaped parking maneuver stage ensure that the vehicle can ultimately park with high precision and parallel alignment, avoiding issues such as vehicle tilting or incomplete parking. Finally, through decoupling and phased control of the parking process, the system exhibits stronger adaptability and robustness to changes in environmental parameters such as parking space length and initial vehicle posture. This invention proposes a complete automatic parking trajectory planning solution through a two-stage partitioning of the parking process, innovative collision avoidance constraint modeling, phased multi-objective optimization, and local hot-start acceleration technologies. This solution systematically addresses several key challenges in parking in narrow spaces, providing new insights for the widespread application of automated parking technology. Attached Figure Description

[0029] Figure 1 This is a flowchart illustrating the multi-step parallel parking trajectory planning based on keyframe partitioning in an embodiment of this application.

[0030] Figure 2 This is a schematic diagram of the vehicle kinematics model established in the embodiments of this application;

[0031] Figure 3 This is a schematic diagram of definition 1 in the embodiments of this application;

[0032] Figure 4 This is a schematic diagram illustrating the limitations of the parking optimization problem without segmentation in the embodiments of this application;

[0033] Figure 5 This is a schematic diagram of definition 3 in the embodiments of this application;

[0034] Figure 6 This is a schematic diagram of the two-stage parking process in the embodiments of this application;

[0035] Figure 7 This is a schematic diagram of the extreme positions during the vehicle parking phase in an embodiment of this application;

[0036] Figure 8 This is a schematic diagram of the planned parking trajectory and control and state parameters in the embodiments of this application. Detailed Implementation

[0037] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] See Figure 1 — Figure 8 The multi-step parallel parking trajectory planning method based on keyframe partitioning includes the following steps in its execution flow:

[0039] S101. Establish the optimal parking control model and determine the initial / target state and vehicle kinematics / collision avoidance constraints:

[0040] Define the parking problem: Based on the definition of the parking trajectory planning problem for autonomous vehicles, As a state space For obstacle space, Free space. Given an initial state. and target state The parking trajectory planning problem can be described as finding a time-parameterized continuous trajectory in a parking scenario. The trajectory planning problem requires the desired trajectory to satisfy:

[0041] 1. Initial and final boundary conditions, i.e. , ;

[0042] 2. The trajectory remains in free space throughout the entire time interval. ;

[0043] 3. Constraints of the vehicle's dynamic model Its mathematical optimization form can be expressed as:

[0044]

[0045] Footprint(x(t)) represents the geometric envelope of the vehicle in state x(t).

[0046] Establishing a vehicle kinematic model: Based on the Ackermann front-wheel steering principle, a vehicle kinematic model under pure rolling conditions is established in the ground coordinate system. The rear axle center is selected as the state observation point for planning the autonomous parking trajectory. The vehicle's state vector and control vector can be expressed as follows:

[0047]

[0048] Where x and y are the coordinates of the rear axis center point; θ is the yaw angle, deg; The velocity at the center point of the rear axle is m / s; φ is the equivalent steering angle of the front wheel, rad. L is the rate of change of front wheel steering angle, rad / s; L is the vehicle length, m; W is the vehicle width, m; L w L is the vehicle wheelbase, in meters (m). f , L r Let be the front and rear overhang lengths of the vehicle, in meters (m); points A, B, C, and D be the endpoints of the vehicle, respectively; the kinematic characteristics of a front-wheel steering vehicle can be described by differential equations:

[0049]

[0050] The established vehicle kinematic model is as follows Figure 2 As shown, the vehicle dimensions, the location of the rear axle center point, and geometric relationships, as well as the horizontal and vertical coordinates of the four contour vertices A, B, C, and D, can be derived as follows:

[0051]

[0052] Constraint Analysis: To ensure that the planned parking trajectory meets the vehicle's physical characteristics, obstacle avoidance requirements, and terminal attitude accuracy, a strict constraint system needs to be constructed from three dimensions: vehicle kinematics, state boundaries, and collision avoidance, as detailed below:

[0053] 1. Vehicle kinematic constraints: Vehicle kinematic constraints are the core prerequisite for trajectory feasibility. They stem from the limitations of the vehicle's own structure and physical characteristics. It is necessary to strictly limit the speed, acceleration, front wheel angle and rate of change of angle during the motion process to avoid exceeding the limits of the vehicle's mechanical performance.

[0054] Based on the Ackermann steering principle and the motion characteristics in low-speed parking scenarios, the following constraints are defined:

[0055] Speed ​​constraint: During parking, the rear axle center speed v of the vehicle must be within a safe range to avoid control instability caused by high speed;

[0056] Acceleration constraints: The longitudinal acceleration 'a' must be limited to a comfortable and safe range to avoid rapid acceleration or deceleration;

[0057] Front wheel steering angle constraint: The equivalent steering angle φ of the front wheels is limited by the mechanical limits of the steering system;

[0058] Front wheel steering angle change rate constraint: Front wheel steering angle change rate It is necessary to limit and avoid the impact on the steering system caused by rapid steering wheel rotation.

[0059] The mathematical unified expression for the above constraints is:

[0060]

[0061] The above formula covers the core physical constraints of vehicle movement and is the basic constraint for trajectory planning, which must be continuously satisfied throughout the entire parking time interval.

[0062] 2. Boundary Constraints for State and Control Variables

[0063] Boundary constraints for state and control variables are divided into initial boundaries and terminal boundaries, used to ensure that the vehicle smoothly transitions from the initial pose to the target parking pose, and that the terminal pose meets the requirements of "parallel positioning and precise alignment". Endpoint state constraints for points and endpoints refer to the constraints on the vehicle at t = 0 and t = t... f Boundary constraints are imposed on the vehicle's state (such as position, attitude, and speed) at the time of travel. These constraints are used to ensure that the vehicle's movement from the starting point to the ending point meets the actual requirements and achieves the goal of safe and smooth parking.

[0064] The initial state must be consistent with the vehicle's actual starting posture, including position, yaw angle, speed, and front wheel steering angle, i.e.:

[0065]

[0066] Among them, (x) start y start Let θ be the initial coordinate of the rear axle center of the vehicle. start The initial yaw angle is collected in real time by onboard sensors (such as GPS and inertial measurement units).

[0067] The terminal status must meet the core requirements of "fully docked and parallel attitude," specifically including:

[0068] Positional constraints: The rear axle center must be located on the longitudinal centerline of the parking space, and the longitudinal position must ensure that the vehicle body is completely inside the parking space (the rear overhang does not exceed the rear boundary of the parking space, and the front overhang does not exceed the front boundary of the parking space).

[0069] Yaw angle constraint: The vehicle body is completely parallel to the length direction of the parking space, with no tilt;

[0070] Speed ​​constraint: The terminal must remain stationary to prevent runaway.

[0071] Steering constraint: The steering wheel returns to center, facilitating subsequent driving out.

[0072] The mathematical unified expression for the above constraints is:

[0073]

[0074] 3. Collision avoidance constraints

[0075] Collision avoidance constraints are crucial for ensuring parking safety. It is essential to ensure that the minimum distance between the vehicle's geometric envelope (vehicle profile) and static obstacles in the environment (such as adjacent vehicles, curbs, and walls) is not less than a safety threshold throughout the entire movement process. This invention employs a convex constraint modeling method based on duality properties to address the problem of non-differentiability of traditional distance constraints, achieving seamless integration with optimal control problems.

[0076] The vehicle's geometric profile is modeled as a polygon, and its spatial posture during motion is accurately described through translation and rotation transformations, thus more realistically reflecting the vehicle's actual shape and dynamic characteristics.

[0077]

[0078] in The vehicle is a polygon. Let be a rotation matrix. It is a translation vector. , It can be calculated from the vehicle dimensions (L, W), position (x, y), and heading angle θ.

[0079] All in the environment Static obstacles can also be modeled using polygons, thus describing collision relationships together with the vehicle's shape within a unified geometric framework:

[0080]

[0081] in , Let be a known matrix, and lm be the number of faces of the Mth obstacle. Although some obstacles may not be convex as a whole, they can always be decomposed into several convex sub-polygons to meet the dual constraint modeling requirements based on convex sets.

[0082] Collision avoidance constraints between vehicles and obstacles can be achieved by ensuring that the minimum distance between each pair of polygons is always greater than or equal to a given threshold d. min ≥ 0 to achieve:

[0083]

[0084] Since the distance constraint function in the above equation is inherently non-differentiable, it is unsuitable for direct use in optimal control or optimization solutions. Therefore, an auxiliary dual variable λ can be introduced for each obstacle m. m With μ m The original non-differentiable distance constraint is equivalently reformulated as a set of differentiable convex constraints:

[0085]

[0086] S102. Based on kinematic feasibility, analyze the path structure of C-type warehousing and S-type warehousing kneading, and construct the feasible solution space:

[0087] In confined spaces (such as around parking spaces), the feasibility of vehicle movement directly determines the space utilization efficiency and flexibility of path planning adjustments. This invention takes a kinematic approach, deeply analyzing the vehicle's basic steering, steering return, and position adjustment behaviors, and constructs a feasible solution space for the optimal parking control problem based on this, providing a solid theoretical foundation for subsequent trajectory planning.

[0088] Definition 1: If the vehicle's steering angle is 0 and the vehicle's width is less than the width of the tubular space, then the vehicle can move to any height within that space, as illustrated in the diagram. Figure 3 As shown.

[0089] This definition reveals the basic mobility of a vehicle under conditions of no angular deviation. When the vehicle is in an ideal state with an angular deviation of 0, it is theoretically feasible to move longitudinally within the tubular space as long as the width of the tubular space is greater than the width of the vehicle. This provides basic spatial constraints for subsequent path planning.

[0090] Definition 2: When the vehicle's heading angle is not 0 and the vehicle's width is less than the width of the tubular space, the vehicle can adjust its heading angle to 0 within the space through a finite number of S-shaped displacements.

[0091] This definition expands the scope of application of Definition 1 by taking into account the actual situation where the vehicle's steering angle deviates. When the vehicle's steering angle is not 0, the steering angle deviance can be gradually eliminated by reasonably designing an S-shaped displacement sequence, eventually restoring the vehicle to the ideal state where the steering angle is 0.

[0092] In situations where space is limited, directly performing basic steering may be impractical due to collisions with obstacles. In such cases, a series of S-shaped "movement" maneuvers can be used to first achieve small lateral (or longitudinal) displacements, gradually releasing the surrounding space, and then retrying the basic steering maneuver to complete attitude recovery.

[0093] Based on this process, the strategy is adjusted iteratively:

[0094] Basic steering → If constrained, perform S-shaped displacement (Definition 1) → Retry basic steering

[0095] Using the above strategy, the vehicle can gradually recover its steering angle in a confined space, eventually bringing it close to 0.

[0096] In parking path planning, vehicles often need to perform complex maneuvers within confined spaces, especially in scenarios where the initial heading is fixed and the target parking space is close to the obstacle boundary, placing higher demands on the geometric continuity and dynamic controllability of the path. For example... Figure 5 As shown, in the optimal parking problem without path segmentation, although the S-shaped path has certain advantages in terms of flexibility, it increases the risk of "in-cut" collisions with obstacles. This makes the S-shaped path more prone to dynamic collisions in continuous time frames, thereby weakening the overall safety of the path and the robustness of the execution process.

[0097] To alleviate the above problems, this paper introduces a C-shaped path structure consisting of two circular arcs in the same direction (e.g., both turning right). This path is composed of circular arc segments with a constant direction and approximately constant curvature. The vehicle's movement always unfolds in the direction "away from the center," thus giving the trajectory of the right endpoint a natural outward expansion tendency, significantly reducing the probability of internal collisions when approaching obstacle boundaries or entering parking space entrances.

[0098] Based on the above characteristics, this invention further proposes "Definition 3": The vehicle constructs a C-shaped motion path through two right-turning arcs with constant curvature. By reasonably adjusting the radius difference between the two arcs, the vehicle can move to any height within this space, as illustrated in the diagram. Figure 5 As shown.

[0099] Consider a typical parking path consisting of two right-turn arcs: a forward right turn and a reverse right turn. We can verify the adjustability and coverage of the C-shaped motion structure for longitudinal displacement by constructing two right-turn C-shaped trajectories.

[0100] First segment: Forward and right-turn trajectory. The vehicle makes a positive right turn with a radius of R1 and an angle of θ1. The end-effector pose increment is:

[0101]

[0102] The second segment: Reverse right-turn trajectory. After the first segment ends, the vehicle, with a heading angle −θ1 as its initial attitude, reverses along a circular arc trajectory in the right-turn direction. To simplify the calculation, this can be equivalent to "traveling the same trajectory forward from the current attitude, but the result needs to be reversed and rotated back to the original coordinate system".

[0103] First, write down the relative displacement vector for this forward movement:

[0104]

[0105] Since the vehicle is reversing and the initial heading is −θ1, the above displacement needs to be rotated around the origin by −θ1 before taking the negative sign (reversing direction):

[0106]

[0107] The vertical increment of the second segment can be calculated as follows:

[0108]

[0109] The total longitudinal displacement can then be calculated as follows:

[0110]

[0111] make Substitute into the above formula:

[0112]

[0113] in It is a variable parameter, and It is a constant with respect to the rotation angle θ1, and is not zero in the range θ1∈(0,π).

[0114] Therefore, when When it approaches positive infinity or negative infinity, It will also tend towards positive infinity or negative infinity, that is:

[0115]

[0116] The vehicle makes two C-shaped right turns, with a fixed steering angle θ1, by adjusting the difference in the radii of the two turns. It allows the vehicle to reach any longitudinal position.

[0117] S103. Define keyframe constraints to divide the parking process into C-type parking (stage 1) and S-type parking (stage 2):

[0118] Based on the differences in path structure and control behavior during actual parking, the parking process is divided into two typical stages: the C-type parking stage and the S-type parking maneuvering stage, as illustrated in the diagram. Figure 6 As shown.

[0119] We extract the features of C-type parking to establish constraints as the first stage of parking. To achieve safe and efficient C-type parking, path planning should satisfy the following feature constraints:

[0120] Feature 1: During the first stage, the front wheel angle of the vehicle remains positive, i.e., it maintains a continuous right turn;

[0121] Feature 2: The final posture must have a good foundation for reversing into a parking space;

[0122] Feature 3: The rear of the vehicle is completely inside the parking space.

[0123] Based on Definition 2, when a vehicle is in the position shown in the diagram, it satisfies the basic conditions for entering the parking space and maximizes the depth of the vehicle within the space. When the length of the parking space is greater than... Figure 7 At the extreme position shown, to facilitate the vehicle's smooth maneuvering within the tubular space, the vehicle's endpoint in the first stage should be as close as possible to the edge of the tubular space to allow sufficient forward adjustment space. Through geometric analysis, the following constraints can be obtained, and this state is defined as a keyframe.

[0124]

[0125] This leads to the following constraints:

[0126]

[0127] in It is the x-coordinate of vehicle point C at the end of stage one. This is the safety threshold.

[0128] Phase 1 must satisfy the following constraints:

[0129]

[0130] If the vehicle cannot be accurately parked in the parking space in one go during the C-type parking stage, or if the parking space is too small or the parking angle is too far off, it is necessary to enter the parking space adjustment stage for fine-tuning.

[0131] S-shaped kneading stage characteristic constraints:

[0132] Feature 1: The vehicle makes reciprocating adjustments within the parking space;

[0133] Feature 2: Fine-tuning of posture is achieved through multiple forward and backward movements;

[0134] Feature 3: Ultimately, the vehicle body is aligned parallel to the parking space line.

[0135] In this stage, the vehicle's movement is confined to the tubular space shown in the figure. This allows us to ignore collision avoidance constraints with the preceding and following parking spaces, transforming the constraints into edge constraints of the tubular section. The main driving characteristic of this stage is the gradual correction of the vehicle's attitude through multiple combinations of forward and backward movements and steering, conforming to the S-shaped path movement mode in Definition 1. Based on Definitions 1 and 2, it can be proven that the vehicle can smoothly transition from its attitude at the end of the C-shaped parking stage to its final attitude that satisfies the parking endpoint constraints, thus ensuring the completeness and feasibility of the parking task. Therefore, the constraints for the second stage can be defined as follows:

[0136]

[0137] S104. Construct multi-objective functions (minimum time / smoothness / attitude) in stages, and transform the optimal control problem into a nonlinear programming problem using the fourth-order Runge-Kutta method:

[0138] To address the different characteristics of the C-type warehousing stage and the S-type warehousing kneading stage, corresponding objective functions were designed to achieve multi-objective optimization of time efficiency and control stability.

[0139] In this invention, the objective function 𝐽 is designed to simultaneously optimize the time efficiency and vehicle control smoothness during the parking process. Specifically, the objective function consists of three parts: a time minimization term, a control input smoothing term, and a special optimization term for the parking phase. The smoothing term includes a weighted sum of squares of acceleration, angular velocity, steering angle, and vehicle speed to ensure the smoothness of the vehicle control process. The design of this objective function can be analyzed from the following aspects:

[0140] First, the time minimization term aims to minimize the total time required for the vehicle to complete the entire parking process, thereby improving parking efficiency.

[0141] Secondly, to improve the smoothness and controllability during parking, acceleration smoothing, front wheel angular velocity smoothing, and vehicle speed smoothing terms are introduced into the objective function. These terms are used to suppress potentially violent acceleration and braking behaviors during parking, and to limit the steering angle and its rate of change, thereby ensuring the smoothness of vehicle movement and the continuity of steering behavior.

[0142] Furthermore, in the parking and maneuvering phases, the objective function incorporates terms to minimize the endpoint direction angle and the number of maneuvers, respectively, aiming to further reduce the time and adjustment costs required for the entire parking operation. The endpoint direction angle minimization term helps ensure the vehicle completes parking with an ideal orientation, reducing the time and number of subsequent direction adjustments, thus improving parking efficiency. Finally, by introducing constraints on vehicle speed and direction changes in the maneuvering phase, the objective function ensures that the vehicle avoids frequent forward and reverse switching during parking. Specifically, The sign of the vehicle's velocity direction is detected between the current and next time steps. If a reversal occurs (i.e., from forward to backward, or vice versa), an additional penalty is applied to the objective function. This design promotes consistent and stable vehicle trajectory, avoids unnecessary gear shifts, and improves overall parking quality. Finally, the objective functions for the first and second stages are obtained:

[0143]

[0144]

[0145] Optimal control problems can often be reformulated as nonlinear programming problems using discretization methods and solved using readily available solvers. Consider setting the final time t... f Discrete into k f +1 part, that is Where Δt is the sampling interval, and the travel time t can be replaced by... The state vector x(t) and control vector u(t) at discrete time point t k The approximate values ​​are expressed as follows: and Where k = 0, 1, ..., N. For each control interval [t] k , t k +1] Using the fourth-order Runge-Kutta method to solve the continuous-time state equations Discretize the data. Given the current state... and control input (Assuming it remains constant within the current interval), next state The calculation process is as follows:

[0146]

[0147] Therefore, the optimal control problem in parking trajectory planning can be reconstructed as a nonlinear programming (NLP) problem. By solving the two-stage NLP problem sequentially and using the endpoint state of the first stage as the initial condition of the second stage, a smooth transition between stages can be ensured, thereby achieving the feasibility and consistency of the overall path.

[0148] S105. Design a local hot start strategy, match the trajectory template library, and provide a high-quality initial solution for the S-shaped rubbing (stage 2):

[0149] To improve the efficiency and practicality of trajectory planning in parallel parking scenarios, this paper proposes a local hot-start-based acceleration strategy for the parking space merging stage, building upon the two-stage parking planning approach. This strategy abandons the traditional global hot-start method that models the entire parking process, instead constructing a dataset specifically for the parking space merging stage, incorporating historical experience trajectories for different parking space lengths during this stage. Build as a trajectory template library Based on the current parking space length L p Find the closest template trajectory to the initial state:

[0150]

[0151] In the two-stage planning strategy based on keyframe partitioning, the vehicle has already been guided into the parking space via a C-shaped parking path before entering the merging stage, and its initial posture is relatively ideal and concentrated. Furthermore, based on Definitions 1 and 2, it is pointed out that the merging stage is characterized by strong structural regularity and high trajectory controllability; the vehicle can achieve fine-tuning of its position and posture through a limited number of S-shaped displacements. Therefore, by constructing only a small number of trajectory templates covering different parking space lengths, combined with a parking space length matching strategy, high-quality hot starts can be achieved in local areas, effectively ensuring the quality of initial values ​​for trajectory optimization and the real-time response capability of the system.

[0152] S106. Sequentially solve the nonlinear programming problems of stage 1 and stage 2 to complete the optimal parking trajectory planning:

[0153] By efficiently solving the discretized optimization problem, a final executable parking trajectory is generated. First, the nonlinear programming problem of the first stage (C-shaped parking) is solved using a solver to obtain the optimal path of the vehicle from the initial state to the keyframe. Then, this keyframe state is used as the precise starting state of the second stage (S-shaped parking), and combined with the local hot-start initial solution provided in step S105, the NLP problem of the second stage is solved. Figure 8 As shown, by solving sequentially and ensuring smooth transitions between stages, this invention ultimately obtains a complete optimal parking trajectory that satisfies all kinematic constraints, collision avoidance constraints, and multi-objective optimization requirements. This trajectory guides the vehicle from its starting position to the final target parking pose in a manner optimal in both time and control stability. The final output trajectory and control sequence will be directly used as inputs to the trajectory tracking control of the automatic parking system.

[0154] This specification details the principle and implementation of the multi-step parallel parking trajectory planning method based on keyframe partitioning of the present invention. However, these specific embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Any equivalent modifications or variations based on the ideas of the present invention should be considered to fall within the protection scope of the present invention. Therefore, the contents listed herein should not be construed as limiting the protection scope of the present invention, which is ultimately determined by the claims.

Claims

1. A two-stage optimal parallel parking trajectory planning method based on keyframe partitioning, characterized in that, The parallel parking path planning method includes the following steps: First, construct a parking scenario model and vehicle kinematic constraints, establish a ground coordinate system with the right rear corner of the parking space as the origin, the length direction of the parking space as the X-axis (pointing to the front of the parking space), the width direction as the Y-axis (pointing to the inside of the parking space), and the center of the rear axle of the vehicle as the state observation point. Define the state vector and control vector; A vehicle kinematic model was established based on the Ackermann front wheel steering principle, and the coordinates of the vehicle contour vertices were derived. Define the constraints: including dynamic constraints on vehicle speed, acceleration, front wheel steering angle and angular velocity, endpoint constraints of initial state and terminal state, and collision avoidance constraints that are transformed into differentiable forms by using dual property modeling and introducing auxiliary variables. The parking process is divided into a C-type entry stage and an S-type repositioning stage: The first stage (C-shaped parking stage) uses a C-shaped trajectory composed of two right-turning arcs in the same direction. The front wheel angle is always positive. The end point of the stage is the key frame. The key frame must meet the constraint that the rear of the vehicle is completely in the parking space and has the basic posture for reversing into the parking space. The second stage (S-shaped rubbing stage) is a tubular space with an S-shaped basic steering structure (two arcs with opposite steering directions and the same radius). Fine posture adjustment is achieved through a reversible displacement strategy. Construct and solve a multi-objective optimization model in stages: The objective function for the first stage includes a time minimization term, a smoothing term of the weighted sum of squares of the control inputs (acceleration, front wheel angular velocity, vehicle speed, and steering angle), and a term for minimizing the final direction angle. The objective function for the second stage includes a time minimization term, a smoothing term for the weighted sum of squares of the control inputs, and a term for minimizing the number of maneuvers; The fourth-order Runge-Kutta method is used to discretize the vehicle kinematic equations, transforming the optimization problem into a nonlinear programming problem to be solved in stages, with the end-of-stage keyframe state of the first stage serving as the initial state of the second stage. Introducing a local hot start strategy: Construct a trajectory template library and collect historical experience trajectories for the parking lot trajectories under different parking space lengths; Based on the current parking space length and the trajectory that is closest to the initial state of the keyframe in the matching template library, the trajectory planning is completed by using it as the initial solution for the second stage of optimization.

2. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 1, characterized in that, The coordinates of the vehicle outline vertices are derived based on the vehicle dimensions.

3. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 2, characterized in that, The collision avoidance constraint modeling is based on the vehicle contour vertices as described in claim 2. The vehicle as a whole is modeled as a polygon with A, B, C, and D as vertices. If the polygon is a non-convex structure, it is decomposed into several convex sub-polygons. This forms a unified geometric modeling framework with the static obstacles in the environment that are also modeled as convex polygons, ensuring the consistency of the collision avoidance constraint calculation.

4. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 3, characterized in that, The specific implementation of "modeling using dual properties and introducing auxiliary variables to transform non-differentiable distance constraints into differentiable forms" is as follows: For the vehicle convex polygon and obstacle convex polygon described in claim 3, an auxiliary variable is introduced, such that "the minimum distance between the vehicle convex polygon and the m-th obstacle convex polygon is greater than or equal to the safety threshold d". min This non-differentiable constraint is transformed into a differentiable convex constraint, ensuring seamless integration with multi-objective optimization models.

5. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 4, characterized in that, The determination of "the rear of the vehicle is completely in the parking space" is based on the right rear vertex C as described in claim 2. The specific determination criterion is that the horizontal coordinate of the right rear vertex C of the vehicle at the key frame is greater than or equal to the horizontal coordinate of the left boundary of the parking space minus the safety threshold, which ensures that the rear of the vehicle is completely in the parking space and avoids the risk of collision with the boundary of the parking space.

6. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 5, characterized in that, The two right-turning arcs in the same direction of the C-shaped trajectory have curvature continuity. By adjusting the radius parameters of the two arcs (radius R1 of the first forward right-turning arc and radius R2 of the second backward right-turning arc) to control the total longitudinal displacement, the longitudinal position requirements of the key frame can be matched.

7. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 6, characterized in that, Both the two arcs of the S-shaped basic steering structure and the arc of the C-shaped trajectory described in claim 6 satisfy the vehicle kinematic model constraints described in claim 1. Furthermore, the two arcs of the S-shaped structure have opposite steering directions and the same radius, are tangent at the midpoint and are symmetrical about the midpoint, ensuring that the vehicle's steering angle returns to its initial state after executing this structure, thus providing a basis for a reversible displacement strategy.

8. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 7, characterized in that, The reversible displacement strategy is implemented based on the S-shaped basic steering structure described in claim 7. The lateral displacement of a single S-shaped trajectory is ∆y, and the longitudinal displacement is ∆x, as shown in the following formula, where L ω (Wheelbase, φ0 is the front wheel steering angle, θ0 is the single-segment circular arc angle), by repeatedly executing the S-shaped motion N times to accumulate longitudinal displacement, arbitrary longitudinal target displacement can be achieved, and fine attitude adjustment can be completed; 。 9. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 8, characterized in that, The "weighted sum of squares of control inputs" in the multi-objective optimization model includes the square terms of acceleration, front wheel angular velocity, vehicle speed, and steering angle as described in claim 1; and the objective function in the second stage additionally includes a penalty term for changes in speed direction. When the vehicle speed direction (forward / backward) is reversed at adjacent moments, an additional penalty is applied to the objective function to avoid frequent gear shifting and improve the smoothness of the parking process.

10. The two-stage optimal parallel parking trajectory planning method based on keyframe partitioning according to claim 9, characterized in that, The trajectories collected by the trajectory template library are all optimized trajectories of the S-shaped rubbing stage as described in claim 8. The sample scenario is limited to "narrow scenario where the parking space length matches the vehicle length". Each sample scenario generates 3 to 5 trajectories and stores discrete state sequences and control sequences, which not only ensures the relevance of the template library, but also reduces redundant data and improves template matching efficiency.

Images