An automatic parking hierarchical trajectory generation method based on PVD and MPC

Abstract

The application discloses a kind of automatic parking hierarchical trajectory generation method based on PVD and MPC, comprising: (1) the reference path that is generated by hybrid A* path planning algorithm to meet the kinematics of vehicle and not collide with obstacle, then segmented reference path is obtained by segmenting reference path;(2) by parallel path planning and speed planning to different segmented reference path to obtain smooth path without collision and curvature limited and speed information constrained by maximum speed and acceleration, then the two are spliced to obtain segmented trajectory;(3) the segmented planning trajectory is connected to obtain the upper layer global planning trajectory;(4) according to the results of upper layer trajectory planning, the MPC method based on linear vehicle kinematics model is used to correct trajectory while ensuring the non-collision characteristics of the corrected trajectory, and the final output collision-free parking trajectory is obtained. By using the application, the real-time, strong controllability and non-collision characteristics of the automatic parking planning trajectory can be improved, so as to realize the safety and comfort of automatic parking.

Description

Technical Field

[0001] This invention belongs to the field of automatic vehicle parking, and in particular relates to a method for generating hierarchical trajectories for automatic parking based on PVD and MPC. Background Technology

[0002] There are two main approaches to trajectory planning: direct and indirect methods. Direct methods can obtain the optimal solution for the trajectory under given constraints and objective function. However, due to the coupling between path and velocity, their ability to adapt to different scenarios is weak, and they typically have long solution times, making it difficult to meet real-time requirements. Indirect methods, because of the decoupling of path and velocity in the planning process, sacrifice the optimality of the final planned trajectory, but they possess strong scenario adaptability.

[0003] There has been considerable research on path planning and speed planning for automated parking. These can be categorized into: 1) sampling-based methods, 2) search-based methods, and 3) optimization-based methods. Sampling-based methods perform well in both high- and low-dimensional spaces, but due to their random sampling nature, they struggle to effectively combine with the vehicle's kinematic model to obtain the shortest path. Search-based methods, thanks to the existence of heuristic functions, can find feasible paths that conform to the vehicle's kinematic model in a relatively short time. Hybrid A* is a good example, and it is widely used in autonomous driving trajectory planning. However, search-based methods do not perform well in real-time in high-dimensional spaces. These two methods can be collectively referred to as heuristic methods. Heuristic methods can generate initial path and speed information, but they are relatively coarse and difficult to use directly for control; therefore, they are usually used for initial solutions. Optimization-based methods can establish an optimization problem based on evaluation metrics and constraints for judging path quality, and obtain the optimal path under these conditions by solving the optimization problem. Evaluation metrics for trajectory quality typically include safety and smoothness, and common constraints include vehicle kinematic constraints and collision-free constraints. The advantage of optimization-based methods is their ability to parameterize various metrics. However, since most constraints on collision avoidance and curvature restriction are nonlinear or even nonconvex, finding the global optimum of the corresponding optimization problem is difficult. The initial solution of nonlinear programming has a significant impact on the final convergence result of the optimization problem. Therefore, optimization-based methods often use curves / discrete point sequences obtained through sampling / search methods as initial solutions. Even so, in most cases, the convergence speed and optimality of the final solution for nonlinear problems are still unsatisfactory. Summary of the Invention

[0004] The purpose of this invention is to solve the problems existing in the prior art and to provide an automatic parking layered trajectory generation method based on PVD and MPC, which can generate collision-free and highly controllable automatic parking trajectories in real time, thereby improving the stability and safety of vehicles during the automatic parking process.

[0005] The specific technical solution adopted in this invention is as follows:

[0006] A hierarchical trajectory generation method for automatic parking based on PVD and MPC is proposed. This method consists of two stages: upper-level trajectory planning based on PVD and lower-level trajectory correction based on MPC. Specifically, it includes the following steps:

[0007] S1. The first stage of executing upper-level trajectory planning: In the feasible domain of the parking scenario, a reference path that conforms to vehicle kinematics and does not collide with obstacles is generated by the hybrid A* path planning algorithm. Then, the reference path is segmented to obtain a segmented reference path.

[0008] S2. The second stage of upper-level trajectory planning: By performing parallel path planning and velocity planning on different segmented reference paths, a smooth path with no collision and limited curvature is obtained, as well as velocity information constrained by maximum velocity and acceleration. Then, the smooth path and velocity information are spliced ​​together to obtain the segmented trajectory.

[0009] S3. The third stage of executing upper-level trajectory planning: Connect the segmented planned trajectories to obtain the upper-level global planned trajectory, and enter the lower-level trajectory correction stage;

[0010] S4. Perform the lower-level trajectory correction stage: Based on the results of the upper-level trajectory planning, the trajectory is corrected using the MPC method based on the linear vehicle kinematics model while ensuring the collision-free characteristics of the corrected trajectory, resulting in the final output collision-free parking trajectory.

[0011] Preferably, in step S1, the obstacle is any area other than the parking space and the lane.

[0012] Preferably, in step S1, the method for segmenting the reference path to obtain the segmented reference path is as follows:

[0013] Using the velocity-direction switching point of the reference path as the segmentation point, the reference path is divided into N segments, and the entire reference path is represented as follows: The number of reference path points in each segment reference path is expressed as follows: The segment reference path of the i-th segment is represented as follows: Representing γ i Chinese q i The coordinates of each segment reference path point.

[0014] Preferably, in step S2, the method for performing parallel path planning and velocity planning on different segmented reference paths is as follows:

[0015] In N parallel computing units, path-velocity decomposition (PVD) trajectory planning is performed synchronously on the segmented reference paths in the entire reference path γ, with each computing unit processing one segmented reference path.

[0016] As a preferred embodiment, the method for generating a collision-free and curvature-constrained smooth path in step S2 is as follows:

[0017] S211. Define the objective function of the quadratic programming:

[0018]

[0019] J2(k)=x′1(s k ) 2 +y′1(s k ) 2

[0020] J3(k)=x″1(s k ) 2 +y″1(s k ) 2

[0021] in, Indicates the segmented reference path γ i The path length from the first reference path point to the kth reference path point, 2≤k≤q i ; x1(·) and y1(·) are both fifth-order polynomials, representing the functions of the change of the x-coordinate and y-coordinate of the smooth path as the vehicle travels a certain distance.

[0022] s k It is the value of the k-th element in vector s. x′ is the number of elements in vector s; i (·), y′ i (·) are the first derivatives of x1(·) and y1(·) with respect to s, respectively; x″1(·) and y″1(·) are the second derivatives of x1(·) and y1(·) with respect to s, respectively.

[0023] S212. Based on the objective function defined in S211, generate a smooth path according to the following quadratic programming problem:

[0024]

[0025]

[0026]

[0027]

[0028] in, It is a unit vector. It is γ i The k-th reference path point; Let ω be the three weight coefficients in the objective function, where ω = [ω(1), ω(2), ω(3)]; The smooth path and γ at the c-th iteration i A vector composed of the fitting coefficients, φ1 represents the k1th fitting coefficient; φ1 and φ2 are both intermediate variables; It is γ i The vehicle yaw angle at the first reference path point in the middle. It is γ i The vehicle yaw angle at the last reference path point; if γ i If it is a forward path, then o p =0, otherwise o p =1;

[0029] S213. Perform collision detection on the smooth path generated in step S212. When a collision occurs, determine the collision fitting coefficient μ. c Calculate the collision fitting coefficient μ for the next iteration. c+1 The calculation method is expressed as follows:

[0030]

[0031] Where μ is composed of all μ j and μ k As elements that together constitute, γ>1; e c It is the diffusion range and e c+1 =e c +1; m is γ i The reference path point index in the reference path point is the path length of the reference path point corresponding to the index that is closest to the path length of the planned path point that collides on the smooth path.

[0032] S214. Repeat steps S212 to S213 until a segmented reference path γ is generated. i Corresponding collision-free smooth path Collision-free smooth path It can be represented in the following form:

[0033]

[0034]

[0035] in, Representing vectors The k-th element in;

[0036] S215. Define the objective function of the quadratic programming:

[0037]

[0038] C2(k)=x′2(s k ) 2 +y′2(s k ) 2

[0039] C3(k)=x″2(s k ) 2 +y″2(s k ) 2

[0040] Where x2(·) and y2(·) are both fifth-order polynomials, representing the functions of the change of the x-coordinate and y-coordinate of the smooth path as the vehicle travels a distance; x′2(·) and y′2(·) are the first derivatives of x2(·) and y2(·) with respect to s, respectively, and x″2(·) and y″2(·) are the second derivatives of x2(·) and y2(·) with respect to s, respectively.

[0041] S216. Based on the objective function defined in S215 and the collision-free smooth path in S214, generate a smooth path according to the following quadratic programming problem:

[0042]

[0043]

[0044]

[0045] in, Yaw angle of planned path points in a smooth path It is approximately a vector composed of adjacent planned path points. with vector The angle formed; These are the three non-zero weights of a quadratic programming problem;

[0046] S217. Based on the quadratic programming problem in S216, a double-loop method is used to generate a collision-free and curvature-constrained smooth path. During the double-loop process, in the c1th collision detection iteration of the outer loop, if the inner loop fails to generate a curvature-constrained smooth path or the smooth path meets the curvature requirements but collides with an obstacle, then the loop is expanded. If the smooth path generated by the inner loop in the c2th iteration cannot meet the curvature requirement, then expand... Weight α(1), α(2), and α(3) are all greater than 1; the double loop continues to run until a smooth path with no collision and limited curvature is obtained or the number of outer loops exceeds the limit and stops.

[0047] S218. If step S217 does not generate a collision-free and curvature-constrained smooth path, then generate an RS curve based on the start and end states of the segmented reference path. If the RS curve has no inflection points, then use the RS curve as the planned path; if the RS curve has inflection points, then at γ... i Uniform interpolation is performed between the coordinates of adjacent reference path points and the reference yaw angle, and the resulting interpolated path state sequence is used as the planned path.

[0048] As a preferred embodiment, the method for generating velocity information constrained by maximum velocity and acceleration in step S2 is as follows:

[0049] S221. Define the quadratic programming problem used to generate velocity information as follows:

[0050]

[0051]

[0052]

[0053]

[0054] l′(t k )≤v m ,|l″(t k )|≤a m

[0055] for k = 2, ...,

[0056] Wherein, the fifth-order polynomial l(·) represents the function of the vehicle's motion path changing with time; l′(·) and l″(·) represent the first and second derivatives of l(·) with respect to time, respectively; t max v represents the maximum running time of the segmented trajectory. m It is the maximum speed of the vehicle, a m It is the vehicle's maximum acceleration; t k It is the value of the k-th element in t. Let be the number of elements in vector t; for the planned path generated in step S217 or S218, the path length from the 1st planned path point to the kth planned path point is expressed as . It represents the number of path points in the planned path;

[0057] S222. Discretize the fifth-degree polynomial l(·) generated in step S221 to obtain the velocity information corresponding to the piecewise planning path as follows:

[0058]

[0059] in, For the planned speed information of the vehicle; when the vehicle is in γ i When the car is in reverse position v =-1, otherwise, o v =1.

[0060] As a preferred embodiment, in step S2, the method for splicing the collision-free and curvature-constrained smooth path with the velocity information constrained by maximum velocity and acceleration is as follows:

[0061] S231, Regarding the planned speed information of vehicles any k-th element Calculate planning speed Corresponding path length:

[0062]

[0063] S232. For the planned path generated in step S217 or S218, use the path length of all path points. composition Find E i All Among the elements, the one that is less than and closest to V is... k numerical value And in the planned path status and A uniform interpolation sequence is obtained by interpolating between them;

[0064] S233, Calculation speed Corresponding front wheel deflection angle The calculation method is expressed as follows:

[0065]

[0066] in, d v It is the path length between adjacent state interpolation points during uniform interpolation in step S232. It is a sequence of yaw angles obtained through interpolation. For sequence The kth i One element; and It is at time k-1. L is the vehicle wheelbase;

[0067] S234, to Repeat steps S231 to S233 for each element to calculate the results. Corresponding front wheel deflection angle Combination The corresponding front wheel deflection angle sequence of the reference trajectory x-coordinate sequence of reference trajectory The ordinate sequence of the reference trajectory Yaw angle sequence of reference trajectory Thus, the segmented planning trajectory is obtained.

[0068] Preferably, in step S3, the method for connecting the segmented planned trajectories is as follows:

[0069] By connecting the segmented planning trajectories obtained from parallel path planning and velocity planning end to end, a higher-level global planning trajectory is obtained.

[0070] As a preferred embodiment, in step S4, the method for ensuring the collision-free characteristics of the corrected trajectory while performing trajectory correction based on MPC is as follows:

[0071] S41. Represent the vehicle's kinematic model using the following differential equation:

[0072]

[0073] Among them, vehicle status x and y are the horizontal and vertical coordinates of the vehicle's rear axle center in the world coordinate system. This is the vehicle's yaw angle; the control law is u = [v δ]. T v represents the longitudinal velocity of the vehicle, and δ represents the front wheel deflection angle of the vehicle;

[0074] S42, Define the matrix

[0075]

[0076] At time k, It is the deviation between the vehicle's condition and the reference condition. It is the deviation between the control law and the reference control law; x r (k) is the reference x-coordinate generated by the upper-level trajectory planning, y r (k) is the reference ordinate generated by the upper-level trajectory planning. It is the reference yaw angle generated by the upper-level trajectory planning, v r (k) is the reference velocity generated by the upper-level trajectory planning, δ r (k) is the reference front wheel deflection angle generated by the upper-level trajectory planning; x(k), y(k), v(k) and δ(k) represent the x-coordinate, yaw angle, speed, and front wheel deflection angle to be solved, respectively.

[0077] S43. Define the change in deviation value as... make Then construct the state equations:

[0078]

[0079] in,

[0080]

[0081]

[0082] Where O represents a matrix of all zeros, I represents an identity matrix, and the subscripts of both matrices indicate the matrix dimension; T represents the time interval for trajectory correction.

[0083] S44. Constructing a quadratic programming problem:

[0084]

[0085]

[0086]

[0087] ∈ min ≤∈≤∈ max ,

[0088]

[0089]

[0090] Where, N p It is the prediction time domain, N c It controls the time domain, ∈ is the slack variable; Q M R and ρ are coefficients of the quadratic programming problem; and These are the lower and upper limits of the change in the deviation value, respectively. and These are the lower and upper limits of the deviation value, respectively. min and ∈ max These are the lower and upper bounds of the slack variable, respectively;

[0091] S45. Solve the quadratic programming problem in step S44 to obtain the solution. Pick The first control law in it is The control law deviation was calculated. The vehicle control law is calculated. Then, the amplitude of u(k) is limited, i.e., v(k)∈[-v m v m ],δ(k)∈[-δ m δ m ], δ m That is the maximum deflection angle of the front wheels;

[0092] S46. Based on the vehicle kinematics model in step S41 and the vehicle control law u(k) obtained in S45, perform a forward simulation to obtain the vehicle state. Then, collision detection is performed on the χ(k) state; when a collision is detected, the coefficient matrix Q is transformed into a quadratic programming problem. M =Q M ·β, β>1, and the time domain is backed up, i.e. k=max(1,kN) p );

[0093] S47. Repeat steps S44 to S46 until the generated vehicle state sequence χ is collision-free. Use the final vehicle state sequence χ as the planned trajectory for automatic parking.

[0094] Compared with the prior art, the present invention has the following beneficial effects:

[0095] 1. This invention first segments the global collision-free path, and then combines parallel computing and PVD trajectory planning methods to perform segmented trajectory planning, thereby improving the computational efficiency of trajectory planning and ensuring the real-time performance of automatic parking trajectory planning.

[0096] 2. In the upper-level trajectory planning, the present invention embeds quadratic planning into the iterative framework for path planning. This not only achieves fast path planning, but also makes the planned path explicitly satisfy collision avoidance constraints and path curvature constraints, thereby improving the safety and controllability of the automatic parking trajectory.

[0097] 3. In the upper-level trajectory planning, the present invention uses a speed planning method based on quadratic programming. This method generates speed information that satisfies the vehicle's maximum speed and maximum acceleration constraints along the previously generated planning path, thereby improving the controllability of the automatic parking trajectory.

[0098] 4. In the lower-level trajectory correction stage, this invention uses a linear vehicle kinematics model to perform trajectory correction using the MPC method while ensuring the collision-free characteristics of the planned trajectory. This improves the controllability of the automatic parking trajectory and ensures the safety and comfort of automatic parking. Attached Figure Description

[0099] Figure 1 This is a flowchart of an automatic parking layered trajectory generation method based on PVD and MPC according to the present invention.

[0100] Figure 2 This is a flowchart of the collision-free and curvature-constrained smooth path generation algorithm in this invention;

[0101] Figure 3 This is a diagram showing the final collision-free parking trajectory generated in this invention.

[0102] Figure 4 This is a diagram showing the planned vehicle speed and front wheel deflection angle data for the final collision-free parking trajectory generated in this invention.

[0103] Figure 5 This is a graph showing the horizontal and vertical coordinates and vehicle yaw angle of the final collision-free parking trajectory generated in this invention.

[0104] Figure 6 This is the reference path generated by the hybrid A* path planning algorithm in this invention. Detailed Implementation

[0105] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be noted that the embodiments described below are intended to facilitate the understanding of the present invention and do not constitute any limitation thereof.

[0106] like Figure 1 As shown, in one embodiment of the present invention, an automatic parking layered trajectory generation method based on PVD and MPC is provided. The method is divided into two stages: upper-layer trajectory planning based on PVD and lower-layer trajectory correction based on MPC. Specifically, it includes the following steps:

[0107] Step 1: Execute the first stage A of the upper-level trajectory planning, specifically as shown in Steps 1-1 to 1-2:

[0108] Step 1-1: Within the feasible region of the parking scenario, a reference path that conforms to the vehicle's kinematics model and avoids collisions with obstacles is generated using the hybridA* path planning algorithm. In this embodiment, obstacles are any area other than the parking space and the lane. The final reference path generated by hybridA* is as follows: Figure 6 As shown.

[0109] Steps 1-2: Segment the reference path to obtain a segmented reference path. The specific segmentation method is as follows: using the velocity-direction switching point of the reference path as the segmentation point, divide the reference path into N segments. The entire reference path is represented as... The number of reference path points in each segment reference path is expressed as follows: Where q i Represents segmented reference path γ i The number of reference path points in the i-th segment is represented as: Representing γ i Chinese q i The coordinates of each segment reference path point.

[0110] Step 2: Execute the second stage B of the upper-level trajectory planning. By performing parallel path planning and velocity planning on different segmented reference paths, a smooth path with no collisions and limited curvature, as well as velocity information constrained by maximum velocity and acceleration, is obtained. Then, the smooth path and velocity information are concatenated to obtain the segmented trajectory. For example... Figure 2 As shown, the specific implementation method of the second stage B is as follows: Steps 2-1 to 2-15:

[0111] Step 2-1: Perform parallel path planning and velocity planning for different segmented reference paths. Specifically, N parallel computing units are set up to perform path-velocity decomposition (PVD) trajectory planning for each segmented reference path within the entire reference path γ. Each computing unit processes one segmented reference path, thus all segmented reference paths can undergo PVD trajectory planning in parallel. The PVD trajectory planning process is detailed in subsequent steps 2-2 to 2-15.

[0112] Step 2-2: Define the objective function of the quadratic programming:

[0113]

[0114] J2(k)=x′ i (s k ) 2 +y′ i (s k ) 2

[0115] J3(k)=x″1(s k ) 2 +y″1(s k ) 2

[0116] in, Indicates the segmented reference path γ iThe path length from the first reference path point to the kth reference path point, 2≤k≤q i ; x1(·) and y1(·) are both fifth-order polynomials, representing the functions of the change of the x-coordinate and y-coordinate of the smooth path as the vehicle travels a certain distance.

[0117] s k It is the value of the k-th element in vector s. Let be the number of elements in vector s; x′1(·) and y′1(·) are the first derivatives of x1(·) and y1(·) with respect to s, respectively, and x″1(·) and y″1(·) are the second derivatives of x1(·) and y1(·) with respect to s, respectively.

[0118] Step 2-3: Based on the objective function defined in Step 2-2, generate a smooth path according to the following quadratic programming problem:

[0119]

[0120]

[0121]

[0122]

[0123] in, It is a unit vector. It is γ i The k-th reference path point in the middle. These are the three weight coefficients in the objective function, i.e., ω = [ω(1), ω(2), ω(3)]. The smooth path and γ at the c-th iteration i A vector composed of the fitting coefficients, φ1 and φ2 represent the k1th fitting coefficient. Both φ1 and φ2 are intermediate variables. It is γ i The vehicle yaw angle at the first reference path point in the middle. It is γ i The vehicle yaw angle at the last reference path point in the middle. If γ i If it is a forward path, then o p =0, otherwise o p =1.

[0124] Step 2-4: Perform collision detection on the smooth path generated in Step 2-3. When a collision occurs, calculate the collision fitting coefficient μ. c Calculate the collision fitting coefficient μ for the next iteration. c+1 The calculation method is expressed as follows:

[0125]

[0126] Where μ is composed of all μ j and μ k As elements collectively constitute μ, which specific element in μ is μ? j Which element is μ? k It depends on the range of values ​​for k and j in the aforementioned expression. γ > 1. e c It is the diffusion range and e c +1 =e c +1, in this embodiment e1 = max(1, round(q) i *0.2)). m is γ i The reference path point index in the system is the one whose path length is closest to the path length of the planned path point that collides on the smooth path.

[0127] Step 2-5: Repeat steps 2-3 to 2-4 until a segmented reference path γ is generated. i Corresponding collision-free smooth path A smooth path without collisions. It can be represented in the following form:

[0128]

[0129]

[0130] in, Representing vectors The k-th element in.

[0131] Step 2-6: Define the objective function of the quadratic programming:

[0132]

[0133] C2(k)=x′2(s k ) 2 +y′2(s k ) 2

[0134] C3(k)=x″2(s k ) 2 +y″2(s k ) 2

[0135] Where x2(·) and y2(·) are both fifth-order polynomials, representing the functions of the change of the horizontal and vertical coordinates of the smooth path as the vehicle travels a distance; x′2(·) and y′2(·) are the first derivatives of x2(·) and y2(·) with respect to s, respectively, and x″2(·) and y″2(·) are the second derivatives of x2(·) and y2(·) with respect to s, respectively.

[0136] Step 2-7: Based on the objective function defined in Step 2-6 and the collision-free smooth path in Step 2-5, generate a smooth path according to the following quadratic programming problem:

[0137]

[0138]

[0139]

[0140] in, Yaw angle of planned path points in a smooth path It is approximately a vector composed of adjacent planned path points. with vector The angle formed, It is a sequence of yaw angles for segmented reference paths. These are the three non-zero weights for a quadratic programming problem.

[0141] Step 2-8: Based on the quadratic programming problem in Step 2-7, a collision-free and curvature-constrained smooth path is generated using a double loop consisting of an inner loop and an outer loop. During the double loop process, in the c1th collision detection iteration of the outer loop, if the inner loop fails to generate a curvature-constrained smooth path, or if the smooth path meets the curvature requirements but collides with an obstacle, the collision is expanded. When the smooth path generated by the c2th iteration of the inner loop cannot meet the curvature requirement, the loop is expanded. Weight It is required that α(1), α(2), and α(3) are all greater than 1. In this embodiment, The initial values ​​are set to w1=2, w2=10, and w3=10 respectively. The double loop continues to run until a smooth path with no collisions and limited curvature is obtained, or the outer loop count exceeds the limit and stops.

[0142] Steps 2-9: If Step 2-8 does not generate a collision-free and curvature-constrained smooth path, then generate a Reeds-Shepp (RS) curve based on the start and end states of the segmented reference path. If the RS curve has no inflection points, then use this RS curve as the planned path; if the RS curve has inflection points, then at γ... iUniform interpolation is performed between the coordinates of adjacent reference path points and the reference yaw angle, and the resulting interpolated path state sequence is used as the planned path.

[0143] Step 2-10: Construct a quadratic programming problem to generate velocity information:

[0144]

[0145]

[0146]

[0147]

[0148] l′(t k )≤v m ,|l″(t k )|≤a m

[0149] for k = 2, ...,

[0150] In this context, the fifth-order polynomial l(·) represents the function of the vehicle's motion path changing with time; l′(·) and l″(·) represent the first and second derivatives of l(·) with respect to time, respectively. max This represents the maximum running time for the segmented trajectory. m =2.5m / s is the maximum speed of the vehicle, a m =1m / s 2 It is the vehicle's maximum acceleration.

[0151] t k It is the value of the k-th element in t. Let be the number of elements in vector t. For the planned path generated in step 2-8 or 2-9, from the first planned path point to the... The path length of each planned path point is expressed as: It represents the number of path points in the planned path.

[0152] Step 2-11: Discretize the fifth-degree polynomial l(·) generated in Step 2-10 to obtain the speed information corresponding to the piecewise planning path, which is expressed as:

[0153]

[0154] in, This provides the planned speed information for the vehicle. When the vehicle is at γ... i When the car is in reverse position v =-1, otherwise, o v =1.

[0155] Step 2-12, for the planned speed information of the vehicle any k-th element Calculate planning speed Corresponding path length:

[0156]

[0157] Step 2-13: For the planned path generated in Step 2-8 or Step 2-9, calculate the path length of all path points. composition Find E i All Among the elements, the one that is less than and closest to V is... k numerical value This determines the index value m of the element and the path planning state. and A sequence of interpolation points is obtained by uniform interpolation between them. Let be the interpolation point sequence. The sequence of x-coordinates of the planned path points. The sequence of ordinates of the planned path points. This is the sequence of yaw angles for the planned path.

[0158] Step 2-14: Calculation speed Corresponding front wheel deflection angle The calculation method is expressed as follows:

[0159]

[0160] in, d v It is the path length between adjacent interpolation points during uniform interpolation in step 2-12. It is a sequence of yaw angles obtained through interpolation. For sequence The kth i Each element. A special one. It is at time k-1. The initial value is L is the vehicle wheelbase. When V k ≈E m hour,

[0161] Step 2-15: [Regarding...] Repeat steps 2-12 to 2-14 for each element to calculate the results. Corresponding front wheel deflection angle Then combine to form The corresponding front wheel deflection angle sequence of the reference trajectory x-coordinate sequence of reference trajectory The ordinate sequence of the reference trajectory Yaw angle sequence of reference trajectory Finally, the segmented planning trajectory is obtained. It should be noted that the above steps will result in a series of segmented planning trajectories.

[0162] Step 3: Connect the path planning obtained by parallel computing with the segmented planning trajectory obtained by velocity planning to obtain the upper-level global planning trajectory.

[0163] Step 4: Perform the lower-level trajectory correction stage:

[0164] Based on the results of the upper-level trajectory planning, the MPC method using a linear vehicle kinematics model is used to correct the trajectory while ensuring the collision-free characteristics of the corrected trajectory, resulting in the final output collision-free parking trajectory. The specific methods are as follows: Steps 4-1 to 4-7.

[0165] Step 4-1: Represent the vehicle kinematic model using the following differential equation:

[0166]

[0167] Among them, vehicle status x and y are the horizontal and vertical coordinates of the vehicle's rear axle center in the world coordinate system. It is the vehicle's yaw angle. Control law. v represents the longitudinal speed of the vehicle, and δ represents the front wheel deflection angle of the vehicle.

[0168] Step 4-2: Define the matrix

[0169]

[0170] At time k, It is the deviation between the vehicle's condition and the reference condition. This is the deviation between the control law and the reference control law. r (k) is the reference x-coordinate generated by the upper-level trajectory planning, y r (k) is the reference ordinate generated by the upper-level trajectory planning. It is the reference yaw angle generated by the upper-level trajectory planning, v r (k) is the reference velocity generated by the upper-level trajectory planning, δ r (k) is the reference front wheel deflection angle generated by the upper-level trajectory planning. x(k), y(k), v(k) and δ(k) represent the x-coordinate, yaw angle, speed, and front wheel deflection angle to be solved, respectively.

[0171] Step 4-3: Let Construct the state equations:

[0172]

[0173] The change in the deviation value is defined as follows:

[0174]

[0175]

[0176] Where O represents a matrix of all zeros, I represents an identity matrix, and the subscripts of both matrices indicate the matrix dimension; T represents the time interval for trajectory correction.

[0177] Step 4-4: Construct the quadratic programming problem:

[0178]

[0179]

[0180]

[0181] ∈ min ≤∈≤∈ max ,

[0182]

[0183]

[0184] Where, N p It is the prediction time domain, N c It controls the time domain, ∈ is the slack variable; Q M R and ρ are coefficients of the quadratic programming problem; and These are the lower and upper limits of the change in the deviation value, respectively. and These are the lower and upper limits of the deviation value, respectively. min and ∈ max These are the lower and upper bounds of the slack variable, respectively. In this embodiment, N p =20, N c =3. Q M = diag([500, 500, 500]), R = diag([5, 5]), the coefficients of the optimization problem ρ = 1.

[0185] Step 4-5: Solve the quadratic programming problem in Step 4-4 to obtain the solution. Pick The first control law in it is The control law deviation was calculated. The vehicle control law is calculated. Then, the amplitude of u(k) is limited, i.e., v(k)∈[-v m v m ],δ(k)∈[-δ m δ m ], δ m This is the maximum deflection angle of the front wheels. In this embodiment, δ m = 0.6 rad.

[0186] Step 4-6: Perform a forward simulation based on the vehicle kinematics model from Step 4-1 and the vehicle control law u(k) obtained in Step 4-5 to obtain the vehicle state. Then, collision detection is performed for the χ(k) state. When a collision is detected, the weight matrix is ​​changed to the coefficients Q of a quadratic programming problem. M =Q M ·β, where β>1 is a weighting coefficient, and the time domain is backed up, i.e., k=max(1,kN) p In this embodiment, β is set to 1.5.

[0187] Steps 4-7: Repeat steps 4-4 to 4-6 until the generated vehicle state sequence χ is collision-free. Use the final vehicle state sequence χ as the planned trajectory for automatic parking. In this embodiment, the final planned collision-free parking trajectory is as follows: Figure 3 As shown. The vehicle speed and front wheel deflection angle data at the final trajectory point are as follows: Figure 4 As shown. The final trajectory point's x-coordinate, y-coordinate, and vehicle yaw angle data are as follows: Figure 5 As shown.

[0188] The embodiments described above provide a detailed explanation of the technical solutions and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, additions, and equivalent substitutions made within the scope of the principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for generating layered trajectories for automatic parking based on PVD and MPC, characterized in that, This method consists of two stages: upper-level trajectory planning based on PVD and lower-level trajectory correction based on MPC. Specifically, it includes the following steps: S1. The first stage of executing upper-level trajectory planning: In the feasible domain of the parking scenario, a reference path that conforms to vehicle kinematics and does not collide with obstacles is generated by the hybrid A* path planning algorithm. Then, the reference path is segmented to obtain a segmented reference path. S2. The second stage of upper-level trajectory planning: By performing parallel path planning and velocity planning on different segmented reference paths, a smooth path with no collision and limited curvature is obtained, as well as velocity information constrained by maximum velocity and acceleration. Then, the smooth path and velocity information are spliced ​​together to obtain the segmented trajectory. In step S2, the method for parallel path planning and velocity planning for different segmented reference paths is as follows: In each parallel computing unit, the entire reference path The segmented reference path in the process is synchronously used for PVD trajectory planning, and each computing unit processes one segmented reference path. In step S2, the method for generating velocity information constrained by maximum velocity and acceleration is as follows: S221. Define the quadratic programming problem used to generate velocity information as follows: Among them, the fifth-order polynomial A function representing the change of the vehicle's path over time; , Represent The first and second derivatives with respect to time; It is the vehicle's maximum speed. It is the vehicle's maximum acceleration; , yes The Middle The value of each element. For vectors The number of elements in The maximum running time for the segmented trajectory; for the generated planned path, from the first planned path point to the... The path length of each planned path point is expressed as: , ; It is the number of waypoints in the planned path; S222, The fifth-degree polynomial generated in step S221 Discretization yields the velocity information corresponding to the segmented planning path, represented as follows: in, For the vehicle's planned speed information; when the vehicle is in When the car is in reverse position ,otherwise, ; S3. The third stage of executing upper-level trajectory planning: Connect the segmented planned trajectories to obtain the upper-level global planned trajectory, and enter the lower-level trajectory correction stage; S4. Perform the lower-level trajectory correction stage: Based on the results of the upper-level trajectory planning, the trajectory is corrected using the MPC method based on the linear vehicle kinematics model while ensuring the collision-free characteristics of the corrected trajectory, resulting in the final output collision-free parking trajectory.

2. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 1, characterized in that, In step S1, the obstacle is any area other than the parking space and the lane.

3. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 1, characterized in that, In step S1, the method for segmenting the reference path to obtain the segmented reference path is as follows: Using the velocity-direction switching point of the reference path as the segmentation point, the reference path is divided into... The segment, the entire reference path is represented as The number of reference path points in each segment reference path is expressed as: , No. Segmentation reference path is represented as , Represent middle The coordinates of each segment reference path point.

4. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 3, characterized in that, In step S2, the method for generating a smooth path with no collisions and limited curvature is as follows: S211. Define the objective function of the quadratic programming: in, Indicates segmented reference path From the first reference path point to the second Path length of each reference path point ; Both are fifth-order polynomials, representing the functions of the change of the x-coordinate and y-coordinate of the smooth path as the vehicle travels a certain distance; , It is a vector The Middle The value of each element. For vectors The number of elements in; They are respectively right The first derivative, They are respectively right The second derivative; S212. Based on the objective function defined in S211, generate a smooth path according to the following quadratic programming problem: in, It is a unit vector. ; yes The Middle One reference path point; These are the three weight coefficients in the objective function. ; It is the first In the next iteration, the smooth path and A vector composed of the fitting coefficients, Representing the One fitting coefficient; and All are intermediate variables; yes The vehicle yaw angle at the first reference path point in the middle. yes The vehicle yaw angle at the last reference path point; if If it is a forward path, then ,otherwise ; S213. Perform collision detection on the smooth path generated in step S212. When a collision occurs, determine the collision fitting coefficient. Calculate the collision fitting coefficient for the next iteration. The calculation method is expressed as follows: in, It is made by all and As elements that together constitute, , ; It is the range of diffusion and ; yes The reference path point index in the reference path point is the path length of the reference path point corresponding to the index that is closest to the path length of the planned path point that collides on the smooth path. S214. Repeat steps S212 to S213 until a segmented reference path is generated. The corresponding collision-free smooth path ; a smooth path without collisions It can be represented in the following form: in, Representing vectors The k-th element in; S215. Define the objective function of the quadratic programming: in, Both are fifth-order polynomials, representing the functions of the change of the x-coordinate and y-coordinate of the smooth path as the vehicle travels a certain distance; They are respectively right The first derivative, They are respectively right The second derivative; S216. Based on the objective function defined in S215 and the collision-free smooth path in S214, generate a smooth path according to the following quadratic programming problem: in, ; Yaw angle of the planned path points in the smooth path Approximately a vector and a vector consisting of adjacent planned path points. The angle formed; , , These are the three non-zero weights of a quadratic programming problem; S217. Based on the quadratic programming problem in S216, a collision-free and curvature-constrained smooth path is generated using a nested loop; during the nested loop process, the outer loop's... During the next collision detection iteration, if the inner loop does not generate a smooth path with limited curvature, or if the smooth path meets the curvature requirements but collides with an obstacle, then the loop is expanded. When the inner loop... If the smooth path generated in the next iteration cannot meet the curvature requirement, then expand... Weight , All values ​​are greater than 1; the double loop continues to run until a smooth path with no collisions and limited curvature is obtained or the number of outer loops exceeds the limit, at which point it stops. S218. If step S217 does not generate a collision-free and curvature-constrained smooth path, then an RS curve is generated based on the start and end states of the segmented reference path. If the RS curve has no inflection points, then this RS curve is used as the planned path; if the RS curve has inflection points, then... Uniform interpolation is performed between the coordinates of adjacent reference path points and the reference yaw angle, and the resulting interpolated path state sequence is used as the planned path.

5. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 4, characterized in that, In step S2, the method for concatenating the smooth path with no collisions and limited curvature and the velocity information constrained by maximum velocity and acceleration is as follows: S231, Regarding the planned speed information of vehicles any k-th element Calculate the planning speed Corresponding path length: S232. For the planned path generated in step S217 or S218, use the path length of all path points. composition ,turn up All Among the elements, the one that is less than and closest to numerical value and in the planned path status and A uniform interpolation sequence is obtained by interpolating between them; S233, Calculation speed Corresponding front wheel deflection angle The calculation method is expressed as follows: in, , It is the path length between adjacent state interpolation points during uniform interpolation in step S232. It is a sequence of yaw angles obtained through interpolation. For sequence The first in One element; and ; yes Moment ; It refers to the vehicle's wheelbase; S234, to Repeat steps S231 to S233 for each element to calculate the results. Corresponding front wheel deflection angle , combined to form The corresponding front wheel deflection angle sequence of the reference trajectory The x-coordinate sequence of the reference trajectory The ordinate sequence of the reference trajectory yaw angle sequence of reference trajectory Thus, the segmented planning trajectory is obtained. ].

6. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 1, characterized in that, In step S3, the method for connecting the segmented planned trajectories is as follows: By connecting the segmented planning trajectories obtained from parallel path planning and velocity planning end to end, a higher-level global planning trajectory is obtained.

7. The automatic parking layered trajectory generation method based on PVD and MPC according to claim 1, characterized in that, In step S4, the method for ensuring the collision-free characteristics of the corrected trajectory while performing trajectory correction based on MPC is as follows: S41. Represent the vehicle's kinematic model using the following differential equation: Among them, vehicle status , These are the x and y coordinates of the vehicle's rear axle center in the world coordinate system. It is the vehicle's yaw angle; control law , Indicates the longitudinal speed of the vehicle. Indicates the front wheel deflection angle of the vehicle; S42, Define the matrix : Among them, time, It is the deviation between the vehicle's condition and the reference condition. It is the deviation between the control law and the reference control law; It is the reference x-coordinate generated by the upper-level trajectory planning. It is the reference ordinate generated by the upper-level trajectory planning. It is the reference yaw angle generated by the upper-level trajectory planning. It is the reference velocity generated by the upper-level trajectory planning. It is the reference front wheel deflection angle generated by the upper-level trajectory planning; , , , , These represent the x-coordinate, yaw angle, speed, and front wheel deflection angle to be solved, respectively. S43. Define the change in deviation value as... ,make Then construct the state equations: in, in, Represents a matrix of all zeros. This represents the identity matrix, and the subscripts of both matrices indicate the matrix dimension. Indicates the time interval for trajectory correction; S44. Constructing a quadratic programming problem: in, It predicts the time domain. It controls the time domain. It is a slack variable; All are coefficients for a quadratic programming problem; and These are the lower and upper limits of the change in the deviation value, respectively. and These are the lower and upper limits of the deviation value, respectively. and These are the lower and upper bounds of the slack variable, respectively; S45. Solve the quadratic programming problem in step S44 to obtain the solution. ,Pick The first control law in it is The control law deviation was calculated. The vehicle's control law is calculated. Then on Limiting the amplitude, i.e. , That is the maximum deflection angle of the front wheels; S46. Based on the vehicle kinematics model in step S41 and the vehicle control law obtained in S45. The vehicle's state is obtained through forward simulation. Then targeting Collision detection is performed on the state; when a collision is detected, the problem is transformed into a quadratic programming problem with a coefficient matrix. And the time domain is rolled back, that is ; S47. Repeat steps S44 to S46 until a vehicle state sequence is generated. It is collision-free, based on the final vehicle state sequence. As the planned trajectory for automatic parking.

Images