Automatic parking track re-planning method suitable for dynamic parking scene

Abstract

The invention relates to the technical field of automatic driving, and particularly discloses an automatic parking track re-planning method suitable for a dynamic parking scene, and the method comprises the steps: detecting the geometric boundary overlapping of a vehicle and an obstacle in real time, determining a collision position, carrying out the coarse path planning through a three-dimensional A * algorithm, determining a coarse path, and carrying out the automatic parking track re-planning. The method comprises the following steps: generating an initial automatic parking feasible trajectory by using maximum acceleration and speed constraint, constructing an optimal control problem containing parameter adaptive collision avoidance constraint, determining a final trajectory objective function of automatic parking, constructing an iterative framework optimization trajectory, adjusting adaptive variables through iteration, gradually approaching strict obstacle avoidance conditions from an initial value, and finally performing automatic parking. And generating a final automatic parking track. The problems that a traditional automatic parking track is difficult to track, the smoothness is insufficient, and the collision avoidance constraint is difficult to meet are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and specifically provides an automatic parking trajectory replanning method applicable to dynamic parking scenarios. Background Art

[0002] With the continuous rapid growth of China's economy and the remarkable improvement of people's living standards, the ownership of motor vehicles has shown an explosive growth trend. However, the limited parking resources and the increasing complexity of the parking environment have made the problem of "difficult parking" increasingly prominent, becoming one of the bottlenecks restricting the development of urban transportation. Against the background of the rapid development of autonomous driving technology, intelligent parking systems provide new ideas for solving this problem. Parking scenarios in dynamic environments are common in life, and the presence of moving obstacles (such as pedestrians, vehicles, etc.) significantly increases the complexity of the planning task. Therefore, studying the real-time planning method of parking trajectories for dynamic scenarios has important theoretical significance and practical value.

[0003] The existing technologies have the following problems:

[0004] Planning method: Global trajectory planning methods are mainly divided into sampling-based, search-based, and optimization-based. Sampling-based and search-based methods plan paths by discretizing the continuous state space into a node graph. However, the paths planned by this method need to be further speed-planned to obtain the driving trajectory, and the trajectories planned by this method may be relatively rugged, 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 inflating obstacles to establish a driving corridor. The method of establishing a cost function cannot strictly ensure that the vehicle does not collide with obstacles, and it is difficult to adjust the parameters in different environments; the method of establishing a driving corridor by inflating obstacles is only applicable to static environments, and in dynamic environments, the three-dimensional space-time corridor will result in a large amount of calculation.

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

[0007] Aiming at the deficiencies of the existing technologies, the present invention provides an automatic parking trajectory replanning method applicable to dynamic parking scenarios, which can effectively solve the problems involved in the above background art.

[0008] To achieve the above objectives, the present invention is realized through the following technical solutions: An automatic parking trajectory replanning method applicable to dynamic parking scenarios, comprising the following steps: In a dynamic parking scenario, based on the original predicted trajectories of the vehicle and obstacles, determine the collision position by real-time detecting the geometric boundary overlap between the vehicle and obstacles; Taking the collision position and the end position of the collision as a reference, determine the starting point and the ending point of the automatic parking replanning, and use the 3D A* algorithm for rough path planning to determine the rough path. The 3D A* algorithm introduces the time dimension as the search space, and the node expansion direction includes spatial displacement and time increment, and optimizes the total path time consumption based on the time penalty term; Perform speed planning on the rough path to generate an initial automatic parking feasible trajectory with maximum acceleration and speed constraints; Establish the kinematic constraints and start-end constraints of the vehicle; Construct an optimal control problem including parameter adaptive collision avoidance constraints, combine the kinematic constraints and start-end constraints of the vehicle, determine the objective function of the final automatic parking trajectory, construct an iterative framework to optimize the trajectory, and gradually approach the strict collision avoidance condition from the initial value by iteratively adjusting the adaptive variables to generate the final automatic parking trajectory.

[0009] As a further method, in a dynamic parking scenario, based on the original predicted trajectories of the vehicle and obstacles, determine the possible collision position by real-time detecting the geometric boundary overlap between the vehicle and obstacles. The specific analysis process is as follows: In a dynamic parking scenario, based on the original predicted trajectory of the vehicle, traverse each time node of the vehicle trajectory, and synchronously obtain the position, heading angle of the vehicle at this moment, and the center coordinates and dimensions of the obstacles; Through the GetVehicleRectangle function, combined with the parameters of the vehicle kinematic model, specifically including the wheelbase, track width, and current heading angle, use the direction cosine matrix to implement coordinate transformation to convert the vehicle contour into a rotated vehicle rectangular bounding box; Construct an axis-aligned obstacle rectangular bounding box according to the original predicted trajectory of the obstacle; Adopt the separating axis theorem algorithm, and use the CheckRectangleCollision function to perform projection overlap detection on the normal vectors of all sides of the vehicle rectangular bounding box and the obstacle rectangular bounding box. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and output all the time points and positions where the collision occurs.

[0010] As a further method, taking the collision position and the end position of the collision as a reference, determine the starting point and the ending point of the automatic parking replanning. The specific analysis process is as follows: Taking the collision position as a reference, select the 5th point forward from its position index value as the starting point of the replanning; Taking the end position of the collision as a reference, select the 5th point backward from its position index value as the ending point of the replanning.

[0011] As a further method, the three-dimensional A* algorithm is used for rough path planning to 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 3 indices [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 time consumption of the path is optimized based on the time penalty term, and the path with the shortest total time consumption is determined as the rough path.

[0012] As a further method, 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 three-dimensional A* algorithm evaluates the priority of nodes 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, which is calculated by accumulating the moving distances between nodes; H(n) is the heuristic cost from the starting point to the current node n, and the Manhattan distance or Euclidean distance is used for quick estimation. Next, an example of calculating the cost of a child node is used:

[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 and child g represent the actual path cost and heuristic cost of the child node, child node(1：2) represents the horizontal and vertical coordinates of the child node, goal node(1：2) represents the horizontal and vertical coordinates of the target node, child node(3) represents the time corresponding to the child node, goal node(3) represents the time corresponding to the target node, cur g represents the actual path cost of the current node, and expansion length represents the cost incurred by the distance traveled from the current node to its child node;

[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 used, and expansion time represents the cost generated by the moving time consumption, which is proportional to the moving time consumption; with the goal of the shortest time consumption for the vehicle to reach the end point of replanning, the total time consumption of the path is optimized.

[0018] As a further method, based on the rough trajectory λ1, all other decision variables are obtained x 0 represents the abscissa of the vehicle, y 0 represents the ordinate of the vehicle, θ 0 represents the yaw angle of the vehicle, represents the time consumed by the trajectory, which is determined by the trajectory length, the maximum speed and acceleration of the vehicle, so that is the shortest, and the speed of each section is set to the maximum value; ν 0 represents the vehicle speed, a 0 represents the vehicle acceleration, φ 0 represents the front wheel steering angle of the vehicle, ω 0 represents the vehicle steering angular velocity, and the calculation method is as follows:

[0019]

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

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

[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, and t f represents the time consumed by the vehicle to reach the destination, v(t) is the longitudinal speed of the vehicle at time t, a(t) is the acceleration of the vehicle at time t, w(t) is the front wheel steering angular velocity 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 angle at time t, and t is the time index;

[0024] The constraint interval is:

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

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

[0027] where a max is the maximum vehicle acceleration, v max is the maximum vehicle longitudinal speed, Ω max is the maximum vehicle front-wheel steering angular velocity, and φ max is the maximum front-wheel steering angle;

[0028] Start and end point constraints:

[0029]

[0030] where X start is the abscissa of the vehicle starting point, y start is the ordinate of the vehicle starting point, θ start is the heading angle of the vehicle starting point, x f is the abscissa of the vehicle end point, yf is the ordinate of the vehicle end point, θ f is the heading angle of the vehicle end point, x(0) is the abscissa of the starting point position, y(0) is the ordinate of the starting point position, θ(0) is the heading angle of the starting point position, v(0) is the vehicle longitudinal speed at the starting point position, φ(0) is the front-wheel steering angle at the starting point position, a(0) is the vehicle acceleration at the starting point position, ω(0) is the vehicle front-wheel steering angular velocity at the starting point position, x(t f ) is the abscissa of the end point position, y(tf) is the ordinate of the end point position, θ(t f ) is the heading angle of the end point position, v(t f ) is the vehicle longitudinal speed at the end point position, φ(t f ) is the front-wheel steering angle at the end point position, a(t f ) is the vehicle acceleration at the end point position, ω(t f ) is the vehicle front-wheel steering angular velocity at the end point position;

[0031] Determine the parameter adaptive collision avoidance constraint, and gradually approach the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variables.

[0032] As a further method, an optimal control problem including parameter - adaptive collision - avoidance constraints is constructed. Combining the kinematic constraints and start - end point constraints of the vehicle, the objective function of the final automatic - parking trajectory is determined. An iterative framework is constructed to optimize the trajectory. By iteratively adjusting the adaptive variables, starting from the initial values, it gradually approaches the strict obstacle - avoidance condition to generate the final automatic - parking trajectory. The specific analysis process is as follows: Two disks are used to cover the rectangular contour of the vehicle, and the radius is denoted as R AGV , and the rectangular obstacle is also regarded as a circle with a radius denoted as r OBS ;

[0033] The parameter - adaptive collision - avoidance constraint is:

[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 center of the obstacle at time t. The continuous trajectory is discretized into a series of trajectory points, that is, [x(N s ), x(N s+1 ),..., x(N s+i )], (x F (i), y F (i)) and (x R (i), y R (i)) respectively represent the coordinates of the centers of the front and rear disks of the vehicle. ∈ is the adaptive variable, t f represents the time taken for the vehicle to reach the destination, k represents the index of the current point, and i is the trajectory - point index;

[0038] According to the geometric dimensions of the vehicle, it can be obtained that:

[0039]

[0040] wherein, L f represents the distance between the front bumper of the vehicle and the front axle, 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 disc to the nearest obstacle must be greater than or at least equal to R AGV :

[0042]

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

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

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

[0046]

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

[0048] As a further method, construct an iterative framework to optimize the trajectory. The specific analysis process is as follows: In each iteration, construct an intermediate optimal control problem, and obtain a new adaptive collision avoidance constraint by using the optimal solution of the previous iteration; The intermediate optimal control problem softens the kinematic constraint and the parameter adaptive collision avoidance constraint and puts them into the cost function as an external penalty cost, and the value of the adaptive variable ∈ increases continuously until it is equal to the original scale constraint;

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

[0050]

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

[0052] The initial value of ∈ is 0.4. When the solution obtained by the optimization is used as the initial solution for the subsequent intermediate optimal control problem, ∈ starts to increase gradually, with an increment of ρ each time. When the optimization is successful after two consecutive increases, the value of ∈ is set to 1. When the optimization fails, ρ is halved, the constraints are relaxed, and the optimization is restarted to obtain a feasible solution. The final automatic parking trajectory is output until the maximum number of iterations is reached 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) By providing an automatic parking trajectory replanning method applicable to dynamic parking scenarios, the present invention can obtain information such as the position, speed, and movement direction of moving obstacles in real time through a sensing module, and predict potential collision times and collision positions based on the vehicle kinematic model and the movement state of obstacles. Secondly, according to the collision prediction results, the starting and ending points of the trajectory to be corrected are accurately located, and the improved three-dimensional A* algorithm is used to quickly replan in a local area, generating an initial path that meets the collision avoidance constraints in a dynamic environment. Finally, combined with the vehicle kinematic constraints and the maximum speed limit, the speed of the initial path is planned to form a feasible initial trajectory.

[0055] (2) By taking the initial trajectory as a reference, the present invention constructs an optimal control problem with multi-objective constraints including safety, comfort, and efficiency, and uses a numerical optimization method to solve it. Secondly, by introducing a parameter adaptive mechanism, the relaxation degree of the collision avoidance constraint is gradually adjusted, transitioning from a loose constraint to a strict constraint, thereby significantly improving the solution efficiency while ensuring the trajectory quality. Finally, a trajectory smoothing algorithm is used to seamlessly splice the replanned trajectory and the original trajectory, generating 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 drawings do not constitute any limitation to the present invention. For those of ordinary skill in the art, other drawings can be obtained based on the following drawings without creative efforts.

[0057] Figure 1 It is a flowchart of an automatic parking trajectory replanning method applicable to dynamic parking scenarios;

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

[0059] Figure 3 It is the pseudocode of the trajectory planning method.

[0060] Figure 4 It is the vehicle motion model diagram.

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

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

[0063] Figure 7 is the variation diagram of the speed, acceleration, yaw angle, and wheel steering angle of the optimized trajectory.

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

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

[0066] Figure 10 It is the longitudinal and lateral error diagram. Specific implementation manner

[0067] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a 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 those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0068] Referring to Figure 1 and Figure 2 As shown, the present invention provides an automatic parking trajectory replanning method applicable to a dynamic parking scenario, including: in a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacles, the positions where collisions occur are determined by real-time detecting the geometric boundary overlap between the vehicle and the obstacles.

[0069] Specifically, in a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacles, the positions where collisions may occur are determined by real-time detecting the overlap of the geometric boundaries between the vehicle and the obstacles. 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 position, heading angle of the vehicle, and the center coordinates and dimensions of the obstacles are synchronously obtained at this moment; through the GetVehicleRectangle function, combined with the vehicle kinematic model parameters, specifically including the wheelbase, track width, and current heading angle, the coordinate transformation is realized using the direction cosine matrix to convert the vehicle contour 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; the Separating Axis Theorem algorithm is adopted, and the projection overlap detection of the normal vectors of all sides of the vehicle rectangular bounding box and the obstacle rectangular bounding box is carried out through the CheckRectangleCollision function. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and all the time points and positions where collisions occur are output.

[0070] Based on the positions where collisions occur and end, the starting point and ending point of the automatic parking replanning are determined, and the three-dimensional A* algorithm is used for rough path planning to determine the rough path. The three-dimensional A* algorithm introduces the time dimension as the search space, the node expansion direction includes spatial displacement and time increment, and the total path time consumption is optimized based on the time penalty term.

[0071] Specifically, based on the positions where collisions occur and end, the starting point and ending point of the automatic parking replanning are determined. The specific analysis process is as follows: Based on the position where the collision occurs, the 5th point forward from its position index value is selected as the starting point of the replanning; based on the position where the collision ends, the 5th point backward from its position index value is selected as the ending point of the replanning.

[0072] Furthermore, the three-dimensional A* algorithm is used for rough path planning to 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, and each node consists of 3 indices [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 consumption is optimized based on the time penalty term, and the path with the shortest total time consumption is determined as the rough path.

[0073] The 3D A* algorithm is highly consistent with the traditional A* algorithm in terms of core idea and implementation framework. Both use the open set and the closed set as node management mechanisms: the open set is used to store nodes to be expanded, while the closed set is used to record the expanded nodes. The execution of the algorithm starts with adding the starting point to the open set, and then iteratively selects the node with the minimum cost from the open set for expansion until the end point is successfully reached or the open set is empty (i.e., the task fails). The 3D A* algorithm not only considers the spatial dimensions (x and y), but also introduces the time dimension (t), enabling path planning in a dynamic environment.

[0074] Specifically, the total time cost 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 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 moving distances between nodes; H(n) is the heuristic cost from the starting point to the current node n, quickly estimated using the Manhattan distance or the Euclidean distance. Next, an example of calculating the cost of a child node is used:

[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 and child g represent the actual path cost and heuristic cost of the child node, child node(1：2) represents the horizontal and vertical coordinates of the child node, goal node(1：2) represents the horizontal and vertical coordinates of the target node, child node(3) represents the time corresponding to the child node, goal node(3) represents the time corresponding to the target node, cur g represents the actual path cost of the current node, and expansion length represents the cost incurred by the distance traveled from the current node to its child node;

[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 used, and expansion time represents the cost generated by the movement time consumption, which is proportional to the movement time consumption; aiming at the shortest time consumption for the vehicle to reach the re-planned end point, the total time consumption of the optimized path is optimized.

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

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

[0082] 2) Monotonicity of the time dimension: During the state expansion process, the time dimension strictly increases monotonically, which is determined by the irreversibility of time. Specifically, the algorithm only allows forward expansion on the time axis, avoiding the physical irrationality of time going backward or stopping;

[0083] 3) In the state expansion in the x-y plane, the 3D A* algorithm allows path points to stay in the time dimension, that is, the vehicle can wait in place to avoid collisions with dynamic obstacles. This mechanism provides the vehicle with higher flexibility, enabling it to continue driving after the obstacle has passed, thus significantly improving the obstacle avoidance ability in dynamic environments.

[0084] The collision-free feasible path generated by the 3D A* algorithm will be used as the input of the speed planning module, and by further optimizing the vehicle's speed curve, the smoothness and executability of the trajectory are ensured.

[0085] As Figure 4 shown, a vehicle motion model is constructed to perform speed planning on the rough path, and an initial automatic parking feasible trajectory is generated with maximum acceleration and speed constraints.

[0086] Specifically, speed planning is performed on the rough path, and an initial automatic parking feasible trajectory is generated with maximum acceleration and speed constraints. The specific analysis process is as follows: Based on the rough trajectory λ1, all other decision variables are obtained x 0 represents the vehicle's abscissa, y 0 represents the vehicle's ordinate, θ 0 represents the vehicle's yaw angle; the trajectory time consumption is determined by the trajectory length, the vehicle's maximum speed and acceleration; such that The shortest one, 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 front wheel steering angle of the vehicle, ω 0 Represents the vehicle steering angular velocity, and the calculation method is as follows:

[0087]

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

[0089] Establish the kinematic constraints and start - end point constraints of the vehicle.

[0090] Specifically, to establish the kinematic constraints and start - end point constraints of the vehicle, the specific analysis process is as follows: Use the classical bicycle model to establish the kinematic constraints of the vehicle:

[0091]

[0092] Among them, x(t), y(t) represent the coordinates of the center point of the vehicle's rear axle at time t, tf represents the time consumed for the vehicle to reach the destination, v(t) is the longitudinal speed of the vehicle at time t, a(t) is the acceleration of the vehicle at time t, w(t) is the front wheel steering angular velocity 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)]′, 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 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 value of the vehicle acceleration, v max is the maximum value of the vehicle longitudinal speed, Ω max is the maximum value of the vehicle front wheel steering angular velocity, φ max is the maximum value of the front wheel angle;

[0097] Start - end point constraints:

[0098]

[0099] Among them, x start is the abscissa of the vehicle starting point, y start is the ordinate of the vehicle starting point, θ start is the orientation angle of the vehicle starting point, x f is the abscissa of the vehicle end point, yf is the ordinate of the vehicle end point, θf is the orientation angle of the vehicle end point, x(0) is the abscissa of the starting point position, y(0) is the ordinate of the starting point position, θ(0) is the orientation angle of the starting point position, v(0) is the longitudinal speed of the vehicle at the starting point position, φ(0) is the front wheel steering angle at the starting point position, a(0) is the vehicle acceleration at the starting point position, ω(0) is the front wheel steering angular speed of the vehicle at the starting point position, x(t f ) is the abscissa of the end point position, y(t f ) is the ordinate of the end point position, θ(t f ) is the orientation angle of the end point position, v(t f ) is the longitudinal speed of the vehicle at the end point position, φ(t f ) is the front wheel steering angle at the end point position, a(t f ) is the vehicle acceleration at the end point position, ω(t f ) is the front wheel steering angular speed of the vehicle at the end point position;

[0100] Determine the parameter adaptive collision avoidance constraint, and gradually approximate the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variable.

[0101] The pseudocode of the trajectory planning method is as Figure 3 shown. Construct an optimal control problem including parameter adaptive collision avoidance constraints, combine the kinematic constraints of the vehicle and the start-end constraints, determine the objective function of the final trajectory for automatic parking, construct an iterative framework to optimize the trajectory, and gradually approximate the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variable to generate the final trajectory for automatic parking.

[0102] In some relatively narrow channels and environments with dynamic obstacles, the collision avoidance constraint may be too strict, which may lead to too long solution time for the optimal control problem or even unable to find a feasible solution. In addition, using a disk to cover the vehicle body and expanding the obstacle map with the disk radius for approximation will ignore some potential drivable areas. This approximation method has errors and has some limitations when used in relatively narrow environments. To solve the above problems, we designed an adaptive collision avoidance constraint to replace the previous collision avoidance constraint.

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

[0104] The parameter adaptive collision avoidance constraint is:

[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] Among them, (x OBS (t), y OBS (t)) represents the position of the center of the obstacle at time t. The continuous trajectory x(i) is discretized into a series of trajectory points, that is, [x(N s ), x(N s+1 ),..., x(N s+i )], (x F (i), y F (i)) and (x R (i), y R (i)) respectively represent the center coordinates of the front and rear disks of the vehicle. ∈ is the adaptive variable, t f represents the time consumed for the vehicle to reach the destination, k represents the index of the current point, and i is the trajectory point index;

[0109] According to the geometric dimensions of the vehicle, it can be obtained that:

[0110]

[0111]

[0112] Among them, L f represents the distance between the front bumper of the vehicle and the front axle, 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 disc to the nearest obstacle must be greater than or at least equal to R AGV :

[0114]

[0115] Among them, L is the wheelbase of the vehicle, and W is the width of the vehicle;

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

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

[0118]

[0119] 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 penalizes the smoothness of the parking trajectory, and the angular velocity ||θ(k + 1) - θ(k)|| 2 penalizes the comfort of the parking trajectory. v(k + 1) is the longitudinal velocity of the vehicle at the (k + 1)-th point, v(k) is the longitudinal velocity of the vehicle at the k-th point, θ(k + 1) is the orientation angle at the (k + 1)-th point, and θ(k) is the orientation angle at the k-th point.

[0120] Specifically, construct an iterative framework to optimize the trajectory. The specific analysis process is as follows: In each iteration, construct an intermediate optimal control problem, and establish a new adaptive collision avoidance constraint through the optimal solution of the previous iteration; the intermediate optimal control problem softens the kinematic constraint and the parameter adaptive collision avoidance constraint and puts them into the cost function as an external penalty cost, and the value of the adaptive variable ∈ increases continuously until it equals the original scale constraint;

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

[0122]

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

[0124]

[0125] In the formula, φ(k) is the front wheel steering angle at the k-th point, a(k) is the vehicle acceleration at the k-th point, ω(k) is the front wheel steering angular velocity of the vehicle at the k-th point, x F (k) is the abscissa at the k-th point, x(k - 1) is the abscissa at the (k - 1)-th point, y F (k) is the ordinate of the obstacle in front at the k-th point, y(k - 1) is the ordinate at the (k - 1)-th point, x R (k) is the abscissa of the obstacle behind at the k-th point, y R (k) is the ordinate of the obstacle behind at the k-th point;

[0126] The initial value of ∈ is 0.4. When the solution obtained by the optimization is to be the initial solution of the next intermediate optimal control problem, ∈ starts to increase gradually, with an increment of ρ each time. When the optimization is 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 carried out again to obtain a feasible solution. The final automatic parking trajectory is output until the maximum number of iterations is reached or the kinematic infeasibility meets the requirements.

[0127] The quality of the initially generated trajectory is relatively low, which can achieve obstacle avoidance but it is difficult for the vehicle to track. In addition, due to the low trajectory quality and the too strict optimization constraints, the optimization may fail. Therefore, a single time based on the initial solution is not always effective. To solve this problem, a while loop is introduced to perform multiple iterative optimizations.

[0128] By predicting the vehicle kinematic model and the obstacle motion state, the collision time and position are calculated in real time, and the 3D A* algorithm is triggered for trajectory replanning. The key of this technology lies in: the collision prediction module: combining the vehicle kinematic constraints and the obstacle motion trajectory to anticipate potential collision risks in advance; the extension of the 3D A* algorithm: introducing the time dimension based on the traditional A* algorithm to support real-time path search in a dynamic environment; the optimization of speed planning: generating the speed curve with the shortest parking time based on the replanned path to ensure the feasibility and efficiency of the trajectory.

[0129] A parameter adaptive mechanism with phased constraint adjustment and multiple iterative optimizations is proposed. Its core technologies include: Constraint relaxation strategy: gradually transitioning from loose obstacle avoidance constraints to strict constraints to balance the solution efficiency and trajectory quality; Multiple rounds of optimization: reducing the dependence on the initial solution through multiple iterative optimizations to increase the probability of the global optimal solution; Trajectory smooth stitching: using the spline interpolation algorithm to seamlessly stitch the replanned trajectory and the original trajectory to generate a continuous and differentiable global trajectory, significantly improving the smoothness and tracking performance of the trajectory.

[0130] To verify the effectiveness of the trajectory planning method proposed in this paper, similar parallel and perpendicular parking spaces were designed by referring to the garage building design code JGJ100 - 2015. Among them, the size of the parallel parking space is 6m×2m, and the size of the perpendicular parking space is 5m×2.4m. The drivable area is represented in white, while the yellow area represents the space occupied by obstacles.

[0131] The specific scenario is as Figure 5 , Figure 6 shown. 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 trajectory of the vehicle is relatively smooth, without colliding with obstacles, and finally reaches the parking space safely. In addition, Figure 7 shows the speed v, acceleration a, yaw angle θ, and steering angle φ of the optimized trajectory, and these values meet the requirements of Table 1 and maintain a certain degree of smoothness.

[0135] To further verify the practicality of the proposed trajectory planning method, a field experiment was carried out on the small autonomous vehicle platform QCar of Quanser. The movement of the vehicle is realized through the throttle and steering servo motors. Six cameras were installed on the ceiling of a 4×4m indoor scenario to reflect the position of the vehicle in real time. The QCar experimental architecture is as Figure 8 shown.

[0136] The trajectory planning module consists of a 3D A* path planning algorithm and a parameter adaptive optimization method, providing 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 speed and position signals to the control module as inputs. Due to experimental conditions, a parallel parking experimental site was built and experimental verification was carried out. Although QCar is smaller in scale compared to a real car, its kinematic model is very close to that of a real-world car model at low speeds. After adjusting several parameters of the trajectory planning algorithm, the algorithm was applied to the QCar platform, and the results were obtained as Figure 9 shown below. Figure 9 The experimental scenario settings are similar to those in the simulation. Among them, the blue line represents the planned path, and the brown line represents the actual trajectory of QCar. QCar successfully reached the parking space without collision. In addition, the close alignment of the planned trajectory and the actual trajectory also proves that the planned trajectory is feasible, smooth, and trackable. Figure 10 Details of the longitudinal and lateral error situations during the experiment are presented. The lateral and longitudinal errors are within an acceptable range, although there are some minor vibrations, which may be attributed to the controller.

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

[0138] The above content is only an example and explanation of the structure of the present invention. Those skilled in the art of this technology can make various modifications or supplements to the described specific embodiments or use similar methods for substitution, as long as they do not deviate from the structure of the invention or exceed the scope defined by this claim book, they should fall within the protection scope of the present invention.

Claims

1. An automatic parking trajectory replanning method applicable to dynamic parking scenarios, characterized in that, It includes the following steps: In a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacles, determine the collision position by real-time detecting the overlap of the geometric boundaries between the vehicle and the obstacles; Taking the collision position and the end position of the collision as a reference, determine the start and end points of the automatic parking replanning, use the 3D A* algorithm for rough path planning to determine the rough path. The 3D A* algorithm introduces the time dimension as the search space, the node expansion direction includes spatial displacement and time increment, and optimizes the total path time consumption based on the time penalty term; Perform speed planning on the rough path to generate an initial automatic parking feasible trajectory with maximum acceleration and speed constraints; Establish the kinematic constraints and start-end point constraints of the vehicle; Construct an optimal control problem including parameter adaptive collision avoidance constraints, combine the kinematic constraints and start-end point constraints of the vehicle, determine the objective function of the final automatic parking trajectory, construct an iterative framework to optimize the trajectory, and gradually approach the strict collision avoidance condition from the initial value by iteratively adjusting the adaptive variables to generate the final automatic parking trajectory.

2. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1, wherein: In a dynamic parking scenario, based on the original predicted trajectories of the vehicle and the obstacles, determine the possible collision position by real-time detecting the overlap of the geometric boundaries between the vehicle and the obstacles. The specific analysis process is as follows: In a dynamic parking scenario, based on the original predicted trajectory of the vehicle, traverse each time node of the vehicle trajectory, and synchronously obtain the position, heading angle of the vehicle, and the center coordinates and dimensions of the obstacles at this moment; Through the GetVehicleRectangle function, combined with the vehicle kinematic model parameters, specifically including the wheelbase, track width, and current heading angle, use the direction cosine matrix to implement coordinate transformation to convert the vehicle contour into a rotated vehicle rectangular bounding box; Construct an axis-aligned rectangular bounding box of the obstacle according to the original predicted trajectory of the obstacle; Adopt the separating axis theorem algorithm, and use the CheckRectangleCollision function to perform projection overlap detection on the normal vectors of all sides of the vehicle rectangular bounding box and the obstacle rectangular bounding box. If there is projection overlap on all separating axes, it indicates that a collision has occurred, and output all the time points and positions where the collision occurs.

3. An automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1, characterized in that: Taking the collision position and the end position of the collision as a reference, determine the start and end points of the automatic parking replanning. The specific analysis process is as follows: Taking the collision position as a reference, select the 5th point forward from its position index value as the start point of the replanning; Taking the end position of the collision as a reference, select the 5th point backward from its position index value as the end point of the replanning.

4. An automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 3, characterized in that: Using the 3D A* algorithm for rough path planning to determine the rough path. The specific analysis process is as follows: Introduce the time dimension 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 is composed of 3 indices [ind1, ind2, ind3]. ind1 corresponds to the x-axis direction, ind2 corresponds to the y-axis direction, 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; Optimize the total path time based on a time penalty term, and determine the rough path as the path with the shortest total time.

5. An automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 4, characterized in that: The specific analysis process for optimizing the total path time based on the time penalty term is as follows: During the path search process, the 3D A* algorithm evaluates the priority of nodes 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 moving distances between nodes; H(n) is the heuristic cost from the starting point to the current node n, quickly estimated using the Manhattan distance or Euclidean distance. Next, an example of calculating the cost of a child node is used: 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 and child g represent the actual path cost and heuristic cost of the child node, child node(1：2) represents the horizontal and vertical coordinates of the child node, goal node(1：2) represents the horizontal and vertical coordinates of the goal node, child node(3) represents the time corresponding to the child node, goal node(3) represents the time corresponding to the goal node, cur g represents the actual path cost of the current node, expansion length represents the cost incurred for the distance traveled from the current node to its child nodes; The cost in 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, and expansion time represents the cost generated by the moving time consumption, which is proportional to the moving time consumption; Optimize the total path time with the goal of minimizing the time it takes for the vehicle to reach the end point of replanning.

6. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 1, wherein: The specific analysis process for performing speed planning on the rough path to generate an initial feasible automatic parking trajectory based on the maximum acceleration and speed constraints is as follows: Based on the rough trajectory λ1, obtain all other decision variables x 0 represents the abscissa of the vehicle, y 0 represents the ordinate of the vehicle, θ 0 represents the yaw angle of the vehicle, represents the time consumed by the trajectory, which is determined by the trajectory length, the maximum speed and acceleration of the vehicle, such that is the shortest, and the speed of each segment is set to the maximum value; v 0 represents the vehicle speed, a 0 represents the vehicle acceleration, φ 0 represents the front wheel steering angle of the vehicle, ω 0 represents the vehicle steering angular velocity, and the calculation method is as follows: Generate an initial feasible automatic parking trajectory λ0, where k represents the index of the current point, n represents the number of all path points, and L is the wheelbase of the vehicle.

7. An automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 6, characterized in that: The specific analysis process for establishing the kinematic constraints and start-end point constraints of the vehicle is as follows: Use the classic bicycle model to establish the kinematic constraints of the vehicle: Among them, x(t) and y(t) represent the coordinates of the center point of the rear axle of the vehicle at time t, tf represents the time it takes for the vehicle to reach the destination, v(t) is the longitudinal speed of the vehicle at time t, a(t) is the acceleration of the vehicle at time t, w(t) is the 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; The constraint interval is: |a(t)| ≤ a max |v(t)| ≤ v max ; |w(t)| ≤ Ω max , |φ(t)| ≤ φ max , t ∈ [0, t f ; where a max is the maximum vehicle acceleration, v max is the maximum vehicle longitudinal speed, Ω max is the maximum front wheel steering angular velocity of the vehicle, and φ max is the maximum front wheel steering angle; Start-end point constraints: Among them, x start is the abscissa of the vehicle starting point, y start is the ordinate of the vehicle starting point, θ start is the orientation angle of the vehicle starting point, x f is the abscissa of the vehicle end point, y f is the ordinate of the vehicle end point, θ f is the orientation angle of the vehicle end point, x(0) is the abscissa of the starting point position, y(0) is the ordinate of the starting point position, θ(0) is the orientation angle of the starting point position, v(0) is the longitudinal speed of the vehicle at the starting point position, φ(0) is the front wheel steering angle at the starting point position, a(0) is the vehicle acceleration at the starting point position, ω(0) is the front wheel steering angular speed of the vehicle at the starting point position, x(t f ) is the abscissa of the end point position, y(t f ) is the ordinate of the end point position, θ(t f ) is the orientation angle of the end point position, v(t f ) is the longitudinal speed of the vehicle at the end point position, φ(t f ) is the front wheel steering angle at the end point position, a(t f ) is the vehicle acceleration at the end point position, ω(t f ) is the front wheel steering angular speed of the vehicle at the end point position; Determine the parameter adaptive collision avoidance constraint, and gradually approach the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variable.

8. An automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 7, characterized in that: The specific analysis process for constructing an optimal control problem including the parameter adaptive collision avoidance constraint, combining the kinematic constraints and start-end point constraints of the vehicle, determining the objective function of the final automatic parking trajectory, constructing an iterative framework to optimize the trajectory, and gradually approaching the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variable to generate the final automatic parking trajectory is as follows: Use two disks to cover the rectangular outline of the vehicle, with the radius denoted as RA GV , and consider the rectangular obstacle as a circle as well, with the radius denoted as r OBS ; The parameter adaptive collision avoidance constraint is: (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 center of the obstacle at time t. The continuous trajectory x(i) is discretized into a series of trajectory points, i.e., [x(N s ), x(N s+1 ),..., x(N s+i )]. (x F (i), y F (i)) and (x R (i), y R (i)) represent the center coordinates of the front and rear discs of the vehicle respectively. ∈ is an adaptive variable, t f represents the time taken for the vehicle to reach the destination, k represents the index of the current point, and i is the trajectory point index; According to the vehicle geometry: Among them, L f represents the distance between the front bumper of the vehicle and the front axle, 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 an obstacle, the distance from the center of each disc to the nearest obstacle must be greater than or at least equal to R AGV : Among them, L is the wheelbase of the vehicle, and W is the width of the vehicle; Determine the objective function of the final automatic parking trajectory, obtain the initial value 0.4 of ∈ stored in the database, use the solver to solve the collision avoidance constraint, construct an iterative framework to optimize the trajectory, and gradually approach the strict obstacle avoidance condition from the initial value by iteratively adjusting the adaptive variable to generate the final automatic parking trajectory.

9. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 8, wherein: The specific analysis process for determining the objective function of the final automatic parking trajectory is as follows: Considering the smoothness, comfort, and parking time of the optimal trajectory, determine the objective function of the final automatic parking trajectory: where μ1>0, μ2>0 and μ3>0 represent weighted parameters, tf penalizes the parking time, and the acceleration ||v(k + 1) - v(k)|| 2 penalizes the smoothness of the parking trajectory, and the angular velocity ||θ(k + 1) - θ(k)|| 2 penalizes the comfort of the parking trajectory, v(k + 1) is the vehicle longitudinal speed at the (k + 1)-th point, ν(k) is the vehicle longitudinal speed at the k-th point, θ(k + 1) is the heading angle at the (k + 1)-th point, and θ(k) is the heading angle at the k-th point.

10. The automatic parking trajectory replanning method applicable to dynamic parking scenarios according to claim 8, wherein: The specific analysis process for constructing an iterative framework to optimize the trajectory is as follows: In each iteration, construct an intermediate optimal control problem, and establish a new adaptive collision avoidance constraint based on the optimal solution of the previous iteration; The intermediate optimal control problem softens the kinematic constraints and the parameter adaptive collision avoidance constraints and puts them into the cost function as external penalty costs. The value of the adaptive variable ∈ is continuously increased until it equals the original scale constraint; minJ + ω p ·(J1 + J2); where ω p is the weight parameter, and J1 and J2 respectively represent the corresponding penalties for kinematic constraints and parameter adaptive collision avoidance constraints; The initial value of ∈ is 0.

4. When the solution obtained by the optimization becomes the initial solution of the next intermediate optimal control problem, ∈ starts to increase gradually, with an increment of ρ each time; When the optimization is successful after two consecutive increases, the value of ∈ will be set to 1 at this time; When the optimization fails, ρ is halved, the constraints are relaxed and the optimization is carried out again to obtain a feasible solution; The final trajectory of the automatic parking is output until the maximum number of iterations is reached or the kinematic infeasibility meets the requirements.

Images