An automatic parking trajectory replanning method for dynamic parking scenarios

Abstract

The present invention relates to the field of autonomous driving technology, specifically disclosing a method for replanning an automated parking trajectory suitable for dynamic parking scenarios. The method comprises: real-time detection of geometric boundary overlap between a vehicle and an obstacle to determine the collision location; coarse path planning using a three-dimensional A* algorithm; determining the coarse path; generating an initial feasible automated parking trajectory with maximum acceleration and velocity constraints; constructing an optimal control problem including parameter-adaptive collision avoidance constraints; determining an objective function for the final automated parking trajectory; constructing an iterative framework to optimize the trajectory; and iteratively adjusting adaptive variables to gradually approach strict obstacle avoidance conditions from initial values ​​to generate the final automated parking trajectory. The present invention addresses the problems of conventional automated parking trajectories, such as difficulty tracking, insufficient smoothness, and difficulty meeting collision avoidance constraints.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to an automatic parking trajectory replanning method suitable for dynamic parking scenarios. Background Art

[0002] With the continued rapid growth of my country's economy and the significant improvement in people's living standards, the number of motor vehicles has shown an explosive growth trend. However, the limited parking resources and the increasingly complex parking environment have made the problem of "difficult parking" increasingly prominent, becoming one of the bottlenecks restricting the development of urban transportation. Against the backdrop of the rapid development of autonomous driving technology, intelligent parking systems provide a new approach to solving this problem. Parking scenarios in dynamic environments are common in our daily lives, and the presence of moving obstacles (such as pedestrians and vehicles) significantly increases the complexity of the planning task. Therefore, studying real-time parking trajectory planning methods for dynamic scenarios has important theoretical significance and practical value.

[0003] The existing technology has the following problems:

[0004] Planning Methods: Global trajectory planning methods are primarily categorized as sampling-based, search-based, and optimization-based. Sampling-based and search-based methods discretize the continuous state space into a node graph to plan a path. However, the paths planned by these methods require further velocity planning to determine the driving trajectory. Furthermore, the resulting trajectory can be bumpy, making it difficult for the vehicle to track.

[0005] Obstacle avoidance constraint establishment: Currently, the mainstream methods for establishing obstacle avoidance constraints are divided into establishing a cost function or expanding obstacles to establish a driving corridor. The cost function method cannot strictly guarantee collision-free vehicles and obstacles, and the parameters are difficult to adjust in different environments. The method of establishing a driving corridor by expanding obstacles is only applicable to static environments. In dynamic environments, the three-dimensional space-time corridor will require a large amount of calculation.

[0006] Therefore, there is an urgent need for a method that can quickly establish obstacle avoidance constraints in a dynamic environment to ensure that collisions with obstacles are avoided. Summary of the Invention

[0007] In view of the shortcomings of the existing technology, the present invention provides an automatic parking trajectory replanning method suitable for dynamic parking scenarios, which can effectively solve the problems involved in the above background technology.

[0008] To achieve the above objectives, the present invention is implemented through the following technical solutions: a method for replanning an automatic parking trajectory suitable for dynamic parking scenarios, comprising the following steps: in a dynamic parking scenario, based on the original predicted trajectories of a vehicle and an obstacle, determining the collision location by real-time detection of the geometric boundary overlap between the vehicle and the obstacle; determining the starting and ending points of the automatic parking replanning based on the collision location and the collision end location as a reference, and performing coarse path planning using a three-dimensional A* algorithm to determine the coarse path. The three-dimensional A* algorithm introduces a time dimension as a search space, the node expansion direction includes spatial displacement and time increment, and the total path time is optimized based on a time penalty term; performing velocity planning on the coarse path, generating an initial feasible automatic parking trajectory based on maximum acceleration and velocity constraints; establishing vehicle kinematic constraints and start and end point constraints; constructing an optimal control problem including parameter adaptive collision avoidance constraints, determining the final automatic parking trajectory objective function in combination with the vehicle kinematic constraints and the start and end point constraints; constructing an iterative framework to optimize the trajectory, and iteratively adjusting adaptive variables to gradually approach strict obstacle avoidance conditions from initial values ​​to generate the final automatic parking trajectory.

[0009] As a further method, in a dynamic parking scenario, based on the original predicted trajectory of the vehicle and the obstacle, the location where a collision may occur is determined by real-time detection of the geometric boundary overlap between the vehicle and the obstacle. The specific analysis process is as follows: in a dynamic parking scenario, based on the original predicted trajectory of the vehicle, each time node of the vehicle trajectory is traversed, and the vehicle position, heading angle, and obstacle center coordinates and dimensions at that moment are synchronously obtained; through the GetVehicleRectangle function, combined with the vehicle kinematic model parameters, including wheelbase, track width, and current heading angle, the direction cosine matrix is ​​used to implement coordinate transformation to convert the vehicle outline into a rotated vehicle rectangular bounding box; an axis-aligned obstacle rectangular bounding box is constructed according to the original predicted trajectory of the obstacle; using the separating axis theorem algorithm, the normal vectors of all sides of the vehicle rectangular bounding box and the obstacle rectangular bounding box are projected and overlapped through the CheckRectangleCollision function. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and the time points and locations of all collisions are output.

[0010] As a further method, the starting and ending points of automatic parking replanning are determined based on the locations where the collision occurred and the collision ended. The specific analysis process is as follows: based on the location where the collision occurred, the fifth point forward from its position index value is selected as the starting point of replanning; based on the location where the collision ended, the fifth point backward from its position index value is selected as the end point of replanning.

[0011] As a further method, the three-dimensional A* algorithm is used for coarse path planning to determine the coarse path. The specific analysis process is: the time dimension is introduced as the search space, and the node expansion direction includes spatial displacement and time increment: the node expansion direction includes spatial displacement and time increment, and each node consists of 3 indexes [ind1, ind2, ind3], ind1 corresponds to the x-axis direction, ind2 corresponds to the y-axis direction, and ind3 corresponds to the time dimension t. At the same time, the time index is in the range of [0, params_NT], and params_NT represents the maximum value of the time dimension; the total path time is optimized based on the time penalty item, and the path with the shortest total time is determined as the coarse path.

[0012] As a further method, the total path time is optimized based on the time penalty term. The specific analysis process is as follows: During the path search process, the three-dimensional A* algorithm evaluates the priority of the node through the cost function F(n) = G(n) + H(n), where G(n) represents the actual path cost from the starting point to the current node n, calculated by accumulating the movement distance between nodes; H(n) is the heuristic cost from the starting point to the current node n, which is quickly estimated using Manhattan distance or Euclidean distance. Next, the cost calculation of a child node is taken as an example:

[0013] child h =sum(child node(1：2) -goal node(1：2) )+w t *abs(child node(3) -goal node(3) )

[0014] child g =cur g +expansion length +expansion time

[0015] Among them, child h with child g Represents the actual path cost and heuristic cost of the child node, child node(1：2) Indicates the horizontal and vertical coordinates of the child node, goal node(1：2) Indicates the horizontal and vertical coordinates of the target node, child node(3) Indicates the time corresponding to the child node, goal node(3) Indicates the time corresponding to the target node, cur g Indicates the actual path cost of the current node, expansion length Indicates the cost of expanding from the current node to its child nodes;

[0016] The cost of the time dimension is: wt *abs(child node(3) -goal node(3) ) and expansion time ;

[0017] Among them, w t is the penalty coefficient for the total time, expansion time It represents the cost of moving time, which is proportional to the moving time. It optimizes the total time of the path with the goal of minimizing the time it takes for the vehicle to reach the replanned destination.

[0018] As a further approach, based on the coarse trajectory λ1, we can obtain all other decision variables x 0 Indicates the vehicle's horizontal coordinate, y 0 represents the vehicle's longitudinal coordinate, θ 0 represents the vehicle yaw angle, It represents the time taken for the trajectory, which is determined by the trajectory length, the maximum speed and acceleration of the vehicle. Shortest, set the speed of each segment to the maximum value; ν 0 represents the vehicle speed, a 0 represents the vehicle acceleration, φ 0 represents the vehicle front wheel steering angle, ω 0 Represents the vehicle's steering angular velocity, which is calculated as follows:

[0019]

[0020] Generate the initial feasible trajectory λ0 for automatic parking, where k represents the index of the current point, n represents the number of all path points, and L is the vehicle wheelbase.

[0021] As a further method, the kinematic constraints and start and end point constraints of the vehicle are established. The specific analysis process is as follows: The kinematic constraints of the vehicle are established using the classic bicycle model:

[0022]

[0023] Among them, x(t), y(t) represent the coordinates of the center point of the rear axle of the vehicle at time t, f represents the time it takes for the vehicle to reach the destination, v(t) is the longitudinal velocity of the vehicle at time t, a(t) is the acceleration of the vehicle at time t, w(t) is the steering angular velocity of the front wheels of the vehicle at time t, L is the wheelbase of the vehicle, the vehicle state X(t) at time t is expressed as [x(t), y(t), θ(t), v(t), φ(t)]′, and the control state U(t) at time t is defined as [a(t), ω(t)]′; θ(t) represents the yaw angle of the vehicle at time t, φ(t) represents the front wheel steering angle at time t, and t is the time index;

[0024] The constraint interval is:

[0025] |a(t)l≤a max ,|v(t)l≤v max ;

[0026] |w(t)|≤Ω max , |φ(t)|≤φ max , t∈[0,t f ];

[0027] Among them, a max , is the maximum acceleration of the vehicle, v max is the maximum longitudinal velocity of the vehicle, Ω max is the maximum steering angular velocity of the vehicle’s front wheels, φ max is the maximum front wheel turning angle;

[0028] Start and end point constraints:

[0029]

[0030] Among them, X start is the horizontal coordinate of the vehicle starting point, y start is the vertical coordinate of the vehicle starting point, θ start is the vehicle starting point heading angle, x f is the abscissa of the vehicle’s endpoint, yf is the ordinate of the vehicle’s endpoint, θ f is the vehicle’s terminal heading angle, x(0) is the starting position’s horizontal coordinate, y(0) is the starting position’s vertical coordinate, θ(0) is the starting position’s heading angle, v(0) is the starting position’s longitudinal velocity, φ(0) is the starting position’s front wheel turning angle, a(0) is the starting position’s vehicle acceleration, ω(0) is the starting position’s front wheel turning angular velocity, x(t f ) is the horizontal coordinate of the end point, y(tf) is the vertical coordinate of the end point, θ(t f ) is the end position orientation angle, v(t f ) is the longitudinal velocity of the vehicle at the end position, φ(t f ) is the front wheel turning angle at the end position, a(t f ) is the vehicle acceleration at the terminal position, ω(t f ) is the steering angular velocity of the vehicle’s front wheels at the final position;

[0031] Determine the parameter adaptive collision avoidance constraints, and gradually approach the strict obstacle avoidance conditions from the initial values ​​by iteratively adjusting the adaptive variables.

[0032] As a further method, an optimal control problem with parameter adaptive collision avoidance constraints is constructed. The kinematic constraints and start and end point constraints of the vehicle are combined to determine the objective function of the final trajectory of the automatic parking. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, the strict obstacle avoidance conditions are gradually approached from the initial values ​​to generate the final trajectory of the automatic parking. The specific analysis process is as follows: two disks are used to cover the rectangular outline of the vehicle, and the radius is recorded as R AGV , the rectangular obstacle is also regarded as a circle, and the radius is recorded as r OBS ;

[0033] The parameter adaptive collision avoidance constraints are:

[0034] (x F (i)-X OBS (t)) 2 +(y F (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ;

[0035] (x R (i)-X OBS (t)) 2 +(y R (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ;

[0036]

[0037] Where (X OBS (t), y OBS (t)) represents the position of the obstacle center at time t, and the continuous trajectory is discretized into a series of trajectory points, namely [x(N s ), x(N s+1 ),...,x(N s+i )],(x F (i), y F (i)) and (x R (i), y R (i)) represents the coordinates of the center of the two disks in front and behind the vehicle, ∈ is an adaptive variable, t f represents the time it takes for the vehicle to reach the destination, k represents the index of the current point, and i is the index of the trajectory point;

[0038] According to the vehicle geometry:

[0039]

[0040] Among them, L f represents the distance between the front bumper and the front axle of the vehicle, Lr represents the distance between the rear bumper and the rear axle, and θ(t) is the yaw angle of the vehicle at time t;

[0041] To ensure that the vehicle does not collide with obstacles, the distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R AGV :

[0042]

[0043] Wherein, L is the vehicle wheelbase and W is the vehicle width;

[0044] Determine the objective function of the final trajectory of automatic parking, obtain the initial value 0.4 of ∈ stored in the database, use the solver to solve the collision avoidance constraints, build an iterative framework to optimize the trajectory, and gradually approach the strict obstacle avoidance conditions from the initial value by iteratively adjusting the adaptive variables to generate the final trajectory of automatic parking.

[0045] As a further method, the final trajectory objective function of automatic parking is determined. The specific analysis process is as follows: considering the smoothness, comfort and parking time of the optimal trajectory, the final trajectory objective function of automatic parking is determined:

[0046]

[0047] Where μ1>0, μ2>0 and μ3>0 represent weighting parameters, tf penalizes the parking time, and the acceleration ||v(k+1)-ν(k)|| 2 Penalize the smoothness of the parking trajectory, the angular velocity ||θ(k+1)-θ(k)|| 2 The comfort of the parking trajectory is penalized, v(k+1) is the longitudinal velocity of the vehicle at the k+1th point, v(k) is the longitudinal velocity of the vehicle at the kth point, θ(k+1) is the heading angle at the k+1th point, and θ(k) is the heading angle at the kth point.

[0048] As a further approach, an iterative framework is constructed to optimize the trajectory. The specific analysis process is as follows: in each iteration, an intermediate optimal control problem is constructed, and a new adaptive collision avoidance constraint is established by obtaining the optimal solution of the previous iteration; the intermediate optimal control problem is put into the cost function as an external penalty cost by softening the kinematic constraint and the parameter adaptive collision avoidance constraint, and the value of the adaptive variable ∈ is continuously increased until it equals the original scale constraint;

[0049] minJ+ω p (J1+J2);

[0050]

[0051] where ω p is the weight parameter, J1 and J2 represent the corresponding penalties of kinematic constraints and parameter adaptive collision avoidance constraints respectively;

[0052] The initial value of ∈ is 0.4. When the optimized solution becomes the initial solution of the next intermediate optimal control problem, ∈ begins to increase gradually, with each increase being ρ. When the optimization solution is still successful after two consecutive increases, the value of ∈ is set to 1. When the optimization fails, ρ is reduced by half, the constraints are relaxed, and the optimization is performed again to obtain a feasible solution. The final trajectory of the automatic parking system is output until the number of iterations reaches the maximum value or the kinematic infeasibility meets the requirements.

[0053] Compared with the prior art, the embodiments of the present invention have at least the following advantages or beneficial effects:

[0054] (1) The present invention provides an automatic parking trajectory replanning method suitable for dynamic parking scenarios. The method obtains information such as the position, speed and movement direction of moving obstacles in real time through a perception module, and predicts the potential collision time and collision position based on the vehicle kinematic model and the obstacle motion state. Secondly, according to the collision prediction results, the starting point and end point of the trajectory to be corrected are accurately located, and the improved three-dimensional A* algorithm is used to perform rapid replanning in the local area, and an initial path that meets the obstacle avoidance constraints is quickly generated in a dynamic environment. Finally, the initial path is speed-planned in combination with the vehicle kinematic constraints and the maximum speed limit to form a feasible initial trajectory.

[0055] (2) The present invention constructs an optimal control problem with multiple constraints including safety, comfort and efficiency by taking the initial trajectory as a reference, and solves it using a numerical optimization method. Secondly, by introducing a parameter adaptive mechanism, the relaxation degree of the obstacle avoidance constraint is gradually adjusted, transitioning from loose constraints to strict constraints, thereby significantly improving the solution efficiency while ensuring the trajectory quality. Finally, the trajectory smoothing algorithm is used to seamlessly splice the replanned trajectory with the original trajectory to generate a global trajectory that meets the collision avoidance constraints and is continuously differentiable, effectively improving the safety and planning efficiency of the parking system in a dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The present invention is further described with reference to the accompanying drawings. However, the embodiments in the accompanying drawings do not constitute any limitation to the present invention. A person skilled in the art can obtain other drawings based on the following drawings without creative effort.

[0057] Figure 1 This is a flow chart of an automatic parking trajectory replanning method applicable to dynamic parking scenarios;

[0058] Figure 2This is the overall framework diagram of the trajectory replanning method.

[0059] Figure 3 Pseudocode for the trajectory planning method.

[0060] Figure 4 This is the motion model diagram of the whole vehicle.

[0061] Figure 5 This is the simulation result of parallel parking in dynamic scenario.

[0062] Figure 6 This is the simulation result of parking in a perpendicular parking space.

[0063] Figure 7 shows the changes in speed, acceleration, yaw angle and wheel steering angle of the optimized trajectory.

[0064] Figure 8 This is the QCar experimental architecture diagram.

[0065] Figure 9 This is the experimental result of the QCar platform.

[0066] Figure 10 The longitudinal and lateral error diagrams are shown. DETAILED DESCRIPTION

[0067] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0068] Reference Figure 1 and Figure 2 As shown, the present invention provides an automatic parking trajectory replanning method suitable for dynamic parking scenarios, including: in a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacle, determining the collision location by real-time detection of the geometric boundary overlap between the vehicle and the obstacle.

[0069] Specifically, in a dynamic parking scenario, based on the original predicted trajectory of the vehicle and the obstacle, the location where a collision may occur is determined by real-time detection of the geometric boundary overlap between the vehicle and the obstacle. The specific analysis process is as follows: in a dynamic parking scenario, based on the original predicted trajectory of the vehicle, each time node of the vehicle trajectory is traversed, and the vehicle position, heading angle, and obstacle center coordinates and size at that moment are synchronously obtained; through the GetVehicleRectangle function, combined with the vehicle kinematic model parameters, including wheelbase, track width, and current heading angle, the direction cosine matrix is ​​used to implement coordinate transformation to convert the vehicle outline into a rotated vehicle rectangular bounding box; an axis-aligned obstacle rectangular bounding box is constructed based on the original predicted trajectory of the obstacle; using the separating axis theorem algorithm, the normal vectors of all sides of the vehicle rectangular bounding box and the obstacle rectangular bounding box are projected and overlapped through the CheckRectangleCollision function. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and the time points and locations of all collisions are output.

[0070] Based on the locations where the collision occurred and ended, the starting and ending points of automatic parking re-planning were determined. The three-dimensional A* algorithm was used for coarse path planning to determine the coarse path. The three-dimensional A* algorithm introduced the time dimension as the search space. The node expansion direction includes spatial displacement and time increment, and the total path time is optimized based on the time penalty term.

[0071] Specifically, the starting and ending points of automatic parking replanning are determined based on the locations where the collision occurred and the collision ended. The specific analysis process is as follows: based on the location where the collision occurred, the fifth point forward from its position index value is selected as the starting point of replanning; based on the location where the collision ended, the fifth point backward from its position index value is selected as the end point of replanning.

[0072] Furthermore, a three-dimensional A* algorithm is used for coarse path planning to determine the coarse path. The specific analysis process is as follows: the time dimension is introduced as the search space, and the node expansion direction includes spatial displacement and time increment: the node expansion direction includes spatial displacement and time increment, and each node consists of 3 indexes [ind1, ind2, ind3], ind1 corresponds to the x-axis direction, ind2 corresponds to the y-axis direction, and ind3 corresponds to the time dimension t; at the same time, the time index is in the range of [0, params_NT], params_dt represents the resolution of the time dimension, and params_NT represents the maximum value of the time dimension; the total path time is optimized based on the time penalty term, and the path with the shortest total time is determined as the coarse path.

[0073] The three-dimensional A* algorithm is highly consistent with the traditional A* algorithm in terms of core concept and implementation framework. Both use open sets (OpenSet) and closed sets (ClosedSet) as node management mechanisms: the open set is used to store nodes to be expanded, while the closed set is used to record expanded nodes. The execution process of the algorithm begins by adding the starting point to the open set, and then iteratively selects the node with the lowest cost from the open set for expansion until the end point is successfully reached or the open set is empty (that is, the task fails). The three-dimensional A* algorithm not only considers the spatial dimensions (x and y), but also introduces the time dimension (t), so that it can plan paths in dynamic environments.

[0074] Specifically, the total path time is optimized based on the time penalty term. The specific analysis process is as follows: During the path search process, the three-dimensional A* algorithm evaluates the priority of the node through the cost function F(n) = G(n) + H(n), where G(n) represents the actual path cost from the starting point to the current node n, calculated by accumulating the movement distance between nodes; H(n) is the heuristic cost from the starting point to the current node n, which is quickly estimated using Manhattan distance or Euclidean distance. Next, the cost calculation of a child node is taken as an example:

[0075] child h =sum(child node(1：2) -goal node(1：2) )+w t *abs(child node(3) -goal node(3) )

[0076] child g =cur g +expansion length +expansion time

[0077] Among them, child h with child g Represents the actual path cost and heuristic cost of the child node, child node(1：2) Indicates the horizontal and vertical coordinates of the child node, goal node(1：2) Indicates the horizontal and vertical coordinates of the target node, child node(3) Indicates the time corresponding to the child node, goal node(3) Indicates the time corresponding to the target node, cur g Indicates the actual path cost of the current node, expansion length Indicates the cost of expanding from the current node to its child nodes;

[0078] The cost of the time dimension is: w t*abs(child node(3) -goal node(3) ) and expansion time ;

[0079] Among them, w t is the penalty coefficient for the total time, expansion time It represents the cost of moving time, which is proportional to the moving time. It optimizes the total time of the path with the goal of minimizing the time it takes for the vehicle to reach the replanned destination.

[0080] Compared with the traditional A* algorithm, the 3D A* algorithm has significant differences in the following aspects:

[0081] 1) The 3D A* algorithm uses time as the third search space, expanding the search dimension of the traditional A* algorithm. Although this improvement increases computational complexity, it enables it to effectively handle path planning tasks in dynamic environments, especially in scenarios where the motion of obstacles changes over time.

[0082] 2) Monotonicity of the time dimension: During state expansion, the time dimension strictly increases monotonically. This property is determined by the irreversibility of time. Specifically, the algorithm only allows forward expansion on the time axis, avoiding the physical implausibility of time reversal or cessation.

[0083] 3) In the xy-plane state expansion, the 3D A* algorithm allows waypoints to pause in the time dimension, allowing the vehicle to wait in place to avoid collisions with dynamic obstacles. This mechanism provides the vehicle with greater flexibility, allowing it to continue driving after obstacles have passed, significantly improving its obstacle avoidance capabilities in dynamic environments.

[0084] The collision-free feasible path generated by the 3D A* algorithm will serve as the input of the speed planning module, which will further optimize the vehicle's speed curve to ensure the smoothness and feasibility of the trajectory.

[0085] like Figure 4 As shown in the figure, the whole vehicle motion model is constructed, the velocity planning is performed on the coarse path, and the initial automatic parking feasible trajectory is generated with the maximum acceleration and velocity constraints.

[0086] Specifically, the speed of the coarse path is planned, and the initial automatic parking feasible trajectory is generated with the maximum acceleration and speed constraints. The specific analysis process is as follows: Based on the coarse trajectory λ1, all other decision variables are obtained x 0 Indicates the vehicle's horizontal coordinate, y 0 represents the vehicle's longitudinal coordinate, θ 0 Indicates the vehicle's yaw angle; the trajectory takes time Determined by the trajectory length, maximum vehicle speed and acceleration; Shortest, set the speed of each segment to the maximum value; v 0 represents the vehicle speed, a 0 represents the vehicle acceleration, φ 0 represents the vehicle front wheel steering angle, ω 0 Represents the vehicle's steering angular velocity, which is calculated as follows:

[0087]

[0088] Generate the initial feasible trajectory λ0 for automatic parking, where k represents the index of the current point, n represents the number of all path points, and L is the vehicle wheelbase.

[0089] Establish the vehicle's kinematic constraints and start and end point constraints.

[0090] Specifically, the kinematic constraints and start and end point constraints of the vehicle are established. The specific analysis process is as follows: Use the classic bicycle model to establish the kinematic constraints of the vehicle:

[0091]

[0092] Where x(t) and y(t) represent the coordinates of the center point of the vehicle's rear axle at time t, tf represents the time it takes for the vehicle to reach its destination, v(t) represents the longitudinal velocity of the vehicle at time t, a(t) represents the acceleration of the vehicle at time t, w(t) represents the steering angular velocity of the front wheels of the vehicle at time t, L represents the wheelbase of the vehicle, the vehicle state X(t) at time t is expressed as [x(t), y(t), θ(t), v(t), φ(t)]′, and the control state U(t) at time t is defined as [a(t), ω(t)]′; θ(t) represents the yaw angle of the vehicle at time t, φ(t) represents the front wheel steering angle at time t, and t is the time index;

[0093] The constraint interval is:

[0094] |a(t)|≤a max ,|v(t)|≤v max ;

[0095] |w(t)|≤Ω max , |φ(t)|≤φ max , t∈[0,t f ];

[0096] Among them, a max , is the maximum acceleration of the vehicle, v max is the maximum longitudinal velocity of the vehicle, Ω max is the maximum steering angular velocity of the vehicle’s front wheels, φ max is the maximum front wheel turning angle;

[0097] Start and end point constraints:

[0098]

[0099] Among them, x start is the horizontal coordinate of the vehicle starting point, y start is the vertical coordinate of the vehicle starting point, θ start is the vehicle starting point heading angle, x f is the vehicle’s endpoint abscissa, yf is the vehicle’s endpoint ordinate, θf is the vehicle’s endpoint heading angle, x(0) is the starting position abscissa, y(0) is the starting position ordinate, θ(0) is the starting position heading angle, v(0) is the vehicle’s longitudinal velocity at the starting position, φ(0) is the front wheel turning angle at the starting position, a(0) is the vehicle’s acceleration at the starting position, ω(0) is the front wheel turning angular velocity at the starting position, x(t f ) is the horizontal coordinate of the end point, y(t f ) is the vertical coordinate of the end point, θ(t f ) is the end position orientation angle, v(t f ) is the longitudinal velocity of the vehicle at the end position, φ(t f ) is the front wheel turning angle at the end position, a(t f ) is the vehicle acceleration at the terminal position, ω(t f ) is the steering angular velocity of the vehicle’s front wheels at the final position;

[0100] Determine the parameter adaptive collision avoidance constraints, and gradually approach the strict obstacle avoidance conditions from the initial values ​​by iteratively adjusting the adaptive variables.

[0101] The pseudo code of trajectory planning method is as follows Figure 3 As shown in the figure, an optimal control problem with parameter adaptive collision avoidance constraints is constructed. The kinematic constraints and start and end point constraints of the vehicle are combined to determine the objective function of the final trajectory of automatic parking. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, the strict obstacle avoidance conditions are gradually approached from the initial values ​​to generate the final trajectory of automatic parking.

[0102] In some relatively narrow passages and environments with dynamic obstacles, collision avoidance constraints can be overly stringent, resulting in excessively long optimal control solution times or even inability to find a feasible solution. Furthermore, approximating the obstacle map by covering the vehicle body with a disk and expanding it by the disk's radius can overlook potentially drivable areas. This approximation is subject to errors and has limitations in confined environments. To address these issues, we designed an adaptive collision avoidance constraint to replace the previous one.

[0103] Specifically, an optimal control problem with parameter adaptive collision avoidance constraints is constructed. The kinematic constraints and start and end point constraints of the vehicle are combined to determine the objective function of the final trajectory of the automatic parking. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, the strict obstacle avoidance conditions are gradually approached from the initial values ​​to generate the final trajectory of the automatic parking. The specific analysis process is as follows: two disks are used to cover the rectangular outline of the vehicle, and the radius is recorded as R. AGV , the rectangular obstacle is also regarded as a circle, and the radius is recorded as r OBS ;

[0104] The parameter adaptive collision avoidance constraints are:

[0105] (x F (i)-X OBS (t)) 2 +(y F (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ;

[0106] (x R (i)-X OBS (t)) 2 +(y R (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ;

[0107]

[0108] Where (x OBS (t), y OBS (t)) represents the position of the obstacle center at time t, and the continuous trajectory x(i) is discretized into a series of trajectory points, namely [x(N s ), x(N s+1 ),...,x(N s+i )],(x F (i), y F (i)) and (x R (i), y R (i)) represents the coordinates of the center of the two disks in front and behind the vehicle, ∈ is an adaptive variable, t f represents the time it takes for the vehicle to reach the destination, k represents the index of the current point, and i is the index of the trajectory point;

[0109] According to the vehicle geometry:

[0110]

[0111]

[0112] Among them, L f represents the distance between the front bumper and the front axle of the vehicle, Lr represents the distance between the rear bumper and the rear axle, and θ(t) is the yaw angle of the vehicle at time t;

[0113] To ensure that the vehicle does not collide with obstacles, the distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R AGV :

[0114]

[0115] Wherein, L is the vehicle wheelbase and W is the vehicle width;

[0116] Determine the objective function of the final trajectory of automatic parking, obtain the initial value 0.4 of ∈ stored in the database, use the solver to solve the collision avoidance constraints, build an iterative framework to optimize the trajectory, and gradually approach the strict obstacle avoidance conditions from the initial value by iteratively adjusting the adaptive variables to generate the final trajectory of automatic parking.

[0117] Furthermore, the final trajectory objective function of the automatic parking is determined. The specific analysis process is as follows: considering the smoothness, comfort and parking time of the optimal trajectory, the final trajectory objective function of the automatic parking is determined:

[0118]

[0119] where μ1>0, μ2>0, and μ3>0 represent weighting parameters, tf penalizes parking time, and acceleration ||v(k+1)-v(k)|‖ 2 Penalize the smoothness of the parking trajectory, the angular velocity ||θ(k+1)-θ(k)|| 2 The comfort of the parking trajectory is penalized, v(k+1) is the longitudinal velocity of the vehicle at the k+1th point, v(k) is the longitudinal velocity of the vehicle at the kth point, θ(k+1) is the heading angle at the k+1th point, and θ(k) is the heading angle at the kth point.

[0120] Specifically, an iterative framework optimization trajectory is constructed. The specific analysis process is as follows: in each iteration, an intermediate optimal control problem is constructed, and a new adaptive collision avoidance constraint is established by obtaining the optimal solution of the previous iteration; the intermediate optimal control problem is put into the cost function as an external penalty cost by softening the kinematic constraint and the parameter adaptive collision avoidance constraint, and the value of the adaptive variable ∈ is continuously increased until it equals the original scale constraint;

[0121] minJ+ωp·(J1+J2);

[0122]

[0123] where ω p is the weight parameter, J1 and J2 represent the corresponding penalties of kinematic constraints and parameter adaptive collision avoidance constraints respectively;

[0124]

[0125] Where φ(k) is the front wheel steering angle at the kth point, a(k) is the vehicle acceleration at the kth point, ω(k) is the front wheel steering angular velocity at the kth point, and x F (k) is the horizontal coordinate of the kth point, x(k-1) is the horizontal coordinate of the k-1th point, y F (k) is the ordinate of the obstacle in front of the k-th point, y(k-1) is the ordinate of the k-1-th point, x R (k) is the horizontal coordinate of the rear obstacle at the kth point, y R (k) is the ordinate of the rear obstacle at the kth point;

[0126] The initial value of ∈ is 0.4. When the optimized solution becomes the initial solution of the next intermediate optimal control problem, ∈ begins to increase gradually, with each increase being ρ. When the optimization solution is still successful after two consecutive increases, the value of ∈ is set to 1. When the optimization fails, ρ is reduced by half, the constraints are relaxed, and the optimization is performed again to obtain a feasible solution. The final trajectory of the automatic parking system is output until the number of iterations reaches the maximum value or the kinematic infeasibility meets the requirements.

[0127] The quality of the initial trajectory generated is relatively low. While it can avoid obstacles, it makes it difficult for the vehicle to track. Furthermore, due to the low trajectory quality and overly strict optimization constraints, the optimization may fail. Therefore, a single optimization based on the initial solution is not always effective. To address this issue, a while loop is introduced to perform multiple iterations of the optimization.

[0128] By combining the vehicle's kinematic model with the predicted obstacle's motion state, the collision time and location are calculated in real time, triggering the 3D A* algorithm for trajectory replanning. Key elements of this technology include: a collision prediction module that combines vehicle kinematic constraints with the obstacle's motion trajectory to anticipate potential collision risks; an expanded 3D A* algorithm that introduces a time dimension to the traditional A* algorithm, supporting real-time path search in dynamic environments; and speed planning optimization that generates a speed curve that minimizes parking time based on the replanned path, ensuring trajectory feasibility and efficiency.

[0129] A parameter adaptation mechanism with phased constraint adjustment and multiple iterative optimization is proposed. Its core technologies include: constraint relaxation strategy: gradually transitioning from loose obstacle avoidance constraints to strict constraints, balancing solution efficiency and trajectory quality; multi-round optimization: reducing dependence on the initial solution through multiple iterative optimization and improving the probability of a global optimal solution; trajectory smoothing: using the spline interpolation algorithm to seamlessly splice the replanned trajectory with the original trajectory to generate a continuously differentiable global trajectory, significantly improving the trajectory smoothness and tracking performance.

[0130] To verify the effectiveness of the proposed trajectory planning method, we designed similar parallel and perpendicular parking spaces based on the garage building design specification (JGJ100-2015). The dimensions of the parallel parking space are 6m × 2m, while the dimensions of the perpendicular parking space are 5m × 2.4m. The drivable area is represented in white, while the yellow area represents the space occupied by obstacles.

[0131] Specific scenarios such as Figure 5 , Figure 6 The key parameter settings of the trajectory planning method are shown in Table 1.

[0132] Table 1: Simulation parameter settings

[0133]

[0134] In this typical parking scenario, the car's trajectory is relatively smooth, with no collisions with obstacles, and it ultimately arrives safely in the parking space. Furthermore, Figure 7 shows the optimized trajectory's velocity v, acceleration a, yaw angle θ, and steering angle φ. These values ​​meet the requirements of Table 1 while maintaining a certain degree of smoothness.

[0135] To further verify the practicality of the proposed trajectory planning method, a field experiment was conducted on a small autonomous vehicle platform QCar from Quanser. The vehicle's movement is achieved through the throttle and steering servo motors. Six cameras were placed on the ceiling of a 4×4 meter indoor scene to reflect the vehicle's position in real time. The QCar experimental architecture is as follows: Figure 8 shown.

[0136] The trajectory planning module consists of a three-dimensional A* path planning algorithm and a parameter adaptive optimization method to provide a feasible trajectory for the tracking control module. Subsequently, a proportional integral (PI) controller is used for longitudinal tracking, and a proportional (P) controller is used for lateral tracking. QCar feeds back the speed and position signals to the control module as input. Due to the limitations of experimental conditions, a parallel parking test site was built and experimental verification was carried out. Although the scale of QCar is smaller than that of a real car, its kinematic model is extremely close to the real-world car model at lower speeds. After adjusting several parameters of the trajectory planning algorithm, the algorithm was applied to the QCar platform, and the following results were obtained: Figure 9 The results shown. Figure 9 The experimental scenario is similar to the one used in the simulation. The blue line represents the planned path, and the brown line represents the QCar's actual trajectory. The QCar successfully reached the parking space without a collision. Furthermore, the close alignment between the planned and actual trajectories demonstrates that the planned trajectory is feasible, smooth, and trackable. Figure 10 The longitudinal and lateral errors in the test are detailed. Both lateral and longitudinal errors are within acceptable ranges, although there are some minor vibrations, which may be attributed to the controller.

[0137] In summary, both simulation and experimental results demonstrate the effectiveness of the trajectory method.

[0138] The above content is merely an example and explanation of the structure of the present invention. Those skilled in the art may make various modifications or additions to the described specific embodiments or replace them in a similar manner. As long as they do not deviate from the structure of the invention or exceed the scope defined by the claims, they should all fall within the scope of protection of the present invention.

Claims

1. An automatic parking trajectory replanning method suitable for dynamic parking scenarios, characterized in that: The following steps are involved: In dynamic parking scenarios, based on the original predicted trajectories of the vehicle and the obstacle, the collision location is determined by real-time detection of the geometric boundary overlap between the vehicle and the obstacle; The starting and ending points of automated parking replanning are determined based on the locations of the collision and the end of the collision. A coarse path is planned using a three-dimensional A* algorithm. The algorithm incorporates the time dimension as the search space, with node expansion directions including spatial displacement and time increments. The algorithm also optimizes the total path duration based on a time penalty. Perform velocity planning on the coarse path to generate the initial feasible trajectory for automated parking with maximum acceleration and velocity constraints; Establish the vehicle's kinematic constraints and start and end point constraints; An optimal control problem with parameter-adaptive collision avoidance constraints is constructed. The objective function of the final automatic parking trajectory is determined by combining the vehicle's kinematic constraints and the start and end point constraints. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, the strict obstacle avoidance conditions are gradually approached from the initial values ​​to generate the final automatic parking trajectory.

2. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1, characterized in that: In a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacle, the possible collision location is determined by real-time detection of the geometric boundary overlap between the vehicle and the obstacle. The specific analysis process is as follows: In dynamic parking scenarios, based on the original predicted trajectory of the vehicle, the system traverses each time node of the vehicle trajectory and synchronously obtains the vehicle's position, heading angle, and obstacle center coordinates and dimensions at that moment. The GetVehicleRectangle function, combined with the vehicle kinematic model parameters, including wheelbase, track width, and current heading angle, uses the direction cosine matrix to implement coordinate transformation to convert the vehicle outline into a rotated vehicle rectangular bounding box. Construct an axis-aligned obstacle rectangular bounding box based on the original predicted obstacle trajectory; Using the separating axis theorem algorithm, the CheckRectangleCollision function is used to perform projection overlap detection on the normal vectors of all sides of the vehicle's rectangular bounding box and the obstacle's rectangular bounding box. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and the time points and locations of all collisions are output.

3. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1 is characterized by: The starting and ending points of the automatic parking replanning are determined based on the locations where the collision occurred and ended. The specific analysis process is as follows: Taking the collision location as the reference, select the fifth point forward from its location index as the starting point for replanning. Based on the position where the collision ends, the fifth point after the position index is selected as the end point of replanning.

4. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 3 is characterized by: The three-dimensional A* algorithm is used to perform rough path planning and determine the rough path. The specific analysis process is as follows: The time dimension is introduced as the search space, and the node expansion direction includes spatial displacement and time increment: The node expansion direction includes spatial displacement and time increment. Each node consists of three indexes [ind1, ind2, ind3], ind1 corresponds to the x-axis direction, ind2 corresponds to the y-axis direction, and ind3 corresponds to the time dimension t. At the same time, the time index is in the range of [0, params_NT], and params_NT represents the maximum value of the time dimension. The total path time is optimized based on the time penalty term, and the path with the shortest total time is determined as the coarse path.

5. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 4 is characterized in that: The total time consumption of the path is optimized based on the time penalty term. The specific analysis process is as follows: During the path search process, the 3D A* algorithm evaluates the priority of nodes using the cost function F(n) = G(n) + H(n), where G(n) represents the actual path cost from the starting point to the current node n, calculated by accumulating the travel distances between nodes; H(n) is the heuristic cost from the starting point to the current node n, quickly estimated using Manhattan distance or Euclidean distance. Next, we use the cost calculation of a child node as an example: child h =sum(child node(1：2) -goal node(1：2) )+w t *abs(child node(3) -goal node(3) ) child g =cur g +expansion length +expansion time Among them, child h with child g Represents the actual path cost and heuristic cost of the child node, child node(1：2) Indicates the horizontal and vertical coordinates of the child node, goal node(1：2) Indicates the horizontal and vertical coordinates of the target node, child node(3) Indicates the time corresponding to the child node, goal node(3) Indicates the time corresponding to the target node, cur g Indicates the actual path cost of the current node, expansion length Indicates the cost of expanding from the current node to its child nodes; The cost of the time dimension is: w t *abs(child node(3) -goal node(3) ) and expansion time ; Among them, w t is the penalty coefficient for the total time, expansion time Indicates the cost of moving time, which is proportional to the time spent on moving; The total time of the route is optimized with the goal of minimizing the time it takes for the vehicle to reach the replanned destination.

6. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1, characterized in that: The speed planning of the rough path is performed to generate the initial feasible trajectory of automatic parking with maximum acceleration and speed constraints. The specific analysis process is as follows: Based on the coarse trajectory λ1, obtain all other decision variables x 0 Indicates the vehicle's horizontal coordinate, y 0 represents the vehicle's longitudinal coordinate, θ 0 represents the vehicle yaw angle, It represents the time taken for the trajectory, which is determined by the trajectory length, the maximum speed and acceleration of the vehicle. The shortest one sets the speed of each segment to the maximum value; v 0 represents the vehicle speed, a 0 represents the vehicle acceleration, φ 0 represents the vehicle front wheel steering angle, ω 0 Represents the vehicle's steering angular velocity, which is calculated as follows: Generate the initial feasible trajectory λ0 for automatic parking, where k represents the index of the current point, n represents the number of all path points, and L is the vehicle wheelbase.

7. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 6, characterized in that: The specific analysis process of establishing the vehicle's kinematic constraints and start and end point constraints is as follows: Use the classic bicycle model to establish the vehicle's kinematic constraints: Where x(t) and y(t) represent the coordinates of the center point of the vehicle's rear axle at time t, tf represents the time it takes for the vehicle to reach its destination, v(t) represents the longitudinal velocity of the vehicle at time t, a(t) represents the acceleration of the vehicle at time t, w(t) represents the steering angular velocity of the front wheels of the vehicle at time t, L represents the wheelbase of the vehicle, the vehicle state X(t) at time t is expressed as [x(t), y(t), θ(t), v(t), φ(t)]′, and the control state U(t) at time t is defined as [a(t), ω(t)]′; θ(t) represents the yaw angle of the vehicle at time t, φ(t) represents the front wheel steering angle at time t, and t is the time index; The constraint interval is: |a(t)|≤a max ,|v(t)|≤v max ; |w(t)|≤Ω max ,|φ(t)|≤φ max ,t∈[0,t f ]; Among them, a max , is the maximum acceleration of the vehicle, v max is the maximum longitudinal velocity of the vehicle, Ω max is the maximum steering angular velocity of the vehicle’s front wheels, φ max is the maximum front wheel turning angle; Start and end point constraints: Among them, x start is the horizontal coordinate of the vehicle starting point, y start is the vertical coordinate of the vehicle starting point, θ start is the vehicle starting point heading angle, x f is the horizontal coordinate of the vehicle's end point, y f is the ordinate of the vehicle’s endpoint, θ f is the vehicle’s terminal heading angle, x(0) is the starting position’s horizontal coordinate, y(0) is the starting position’s vertical coordinate, θ(0) is the starting position’s heading angle, v(0) is the starting position’s longitudinal velocity, φ(0) is the starting position’s front wheel turning angle, a(0) is the starting position’s vehicle acceleration, ω(0) is the starting position’s front wheel turning angular velocity, x(t f ) is the horizontal coordinate of the end point, y(t f ) is the vertical coordinate of the end point, θ(t f ) is the end position orientation angle, v(t f ) is the longitudinal velocity of the vehicle at the end position, φ(t f ) is the front wheel turning angle at the end position, a(t f ) is the vehicle acceleration at the terminal position, ω(t f ) is the steering angular velocity of the vehicle’s front wheels at the end position; Determine the parameter adaptive collision avoidance constraints, and gradually approach the strict obstacle avoidance conditions from the initial values ​​by iteratively adjusting the adaptive variables.

8. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 7, characterized in that: The optimal control problem with parameter adaptive collision avoidance constraints is constructed. The objective function of the final trajectory of the automatic parking is determined by combining the vehicle's kinematic constraints and the start and end point constraints. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, the strict obstacle avoidance conditions are gradually approached from the initial values ​​to generate the final trajectory of the automatic parking. The specific analysis process is as follows: Use two disks to cover the rectangular outline of the vehicle, with radius RA GV , the rectangular obstacle is also regarded as a circle, and the radius is recorded as r OBS ; The parameter adaptive collision avoidance constraints are: (x F (i)-x OBS (t)) 2 +(y F (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ; (x R (i)-x OBS (t)) 2 +(y R (i)-y OBS (t)) 2 ≥∈*(R AGV +r OBS ) 2 ; Where (x OBS (t), y OBS (t)) represents the position of the obstacle center at time t, and the continuous trajectory x(i) is discretized into a series of trajectory points, namely [x(N s ), x(N s+1 ),...,x(N s+i )],(x F (i), y F (i)) and (x R (i), y R (i)) represents the coordinates of the center of the two disks in front and behind the vehicle, ∈ is an adaptive variable, t f represents the time it takes for the vehicle to reach the destination, k represents the index of the current point, and i is the index of the trajectory point; According to the vehicle geometry: Among them, L f represents the distance between the front bumper and the front axle of the vehicle, Lr represents the distance between the rear bumper and the rear axle, and θ(t) is the yaw angle of the vehicle at time t; To ensure that the vehicle does not collide with obstacles, the distance from the center of each disk to the nearest obstacle must be greater than or at least equal to R AGV : Wherein, L is the vehicle wheelbase and W is the vehicle width; Determine the objective function of the final trajectory of automatic parking, obtain the initial value 0.4 of ∈ stored in the database, use the solver to solve the collision avoidance constraints, build an iterative framework to optimize the trajectory, and gradually approach the strict obstacle avoidance conditions from the initial value by iteratively adjusting the adaptive variables to generate the final trajectory of automatic parking.

9. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 8, characterized in that: The specific analysis process of determining the final trajectory objective function of automatic parking is as follows: Taking into account the smoothness, comfort, and parking time of the optimal trajectory, the objective function of the final trajectory of automatic parking is determined: Where μ1>0, μ2>0 and μ3>0 represent weighting parameters, tf penalizes the parking time, and the acceleration ||v(k+1)-v(k)|| 2 Penalize the smoothness of the parking trajectory, the angular velocity ||θ(k+1)-θ(k)|| 2 The comfort of the parking trajectory is penalized, v(k+1) is the longitudinal velocity of the vehicle at the k+1th point, ν(k) is the longitudinal velocity of the vehicle at the kth point, θ(k+1) is the heading angle at the k+1th point, and θ(k) is the heading angle at the kth point.

10. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 8, characterized in that: The specific analysis process of constructing the iterative framework optimization trajectory is as follows: In each iteration, an intermediate optimal control problem is constructed, and new adaptive collision avoidance constraints are established through the optimal solution of the previous iteration; The intermediate optimal control problem is solved by softening the kinematic constraints and the parameter adaptive collision avoidance constraints as external penalty costs into the cost function, and the value of the adaptive variable ∈ increases continuously until it is equal to the original scale constraint; minJ+ω p ·(J1+J2); where ω p is the weight parameter, J1 and J2 represent the corresponding penalties of kinematic constraints and parameter adaptive collision avoidance constraints respectively; The initial value of ∈ is 0.

4. When the optimized solution becomes the initial solution of the next intermediate optimal control problem, ∈ begins to increase gradually, with each increase being ρ. If the optimization solution is still successful after two consecutive increases, the value of ∈ will be set to 1; When the optimization fails, ρ is reduced by half, the constraints are relaxed and the optimization is repeated to obtain a feasible solution; The final trajectory of automatic parking is output until the number of iterations reaches the maximum or the kinematic infeasibility meets the requirements.

Images