An automatic parking path planning method based on quadratic programming and parallel iteration

Abstract

The present invention discloses an automatic parking path planning method based on quadratic programming and parallel iteration, comprising: (1) generating an initial path that conforms to a vehicle kinematic model and does not collide with obstacles using a hybrid A* algorithm; (2) decomposing the initial path according to speed direction switching points in the initial path to obtain a segmented initial path set; (3) constructing a collision avoidance quadratic programming problem based on segmented path elements in the segmented initial path set, and obtaining a segmented collision-free continuous path set corresponding to the segmented initial path set by dynamically adjusting the weights of path points in parallel iteration; (4) constructing a collision avoidance and curvature optimization quadratic programming problem, and then obtaining a collision-free parking path that meets curvature requirements by dynamically adjusting the weights of some objective functions in the optimization target in parallel iteration. Utilizing the present invention, the safety, stability, and comfort of a vehicle during automatic parking can be improved.

Description

Technical Field

[0001] The present invention belongs to the field of automatic vehicle parking, and in particular relates to an automatic parking path planning method based on quadratic programming and parallel iteration. Background Art

[0002] Automatic parking comprises numerous modules, including perception, planning, and control. The perception module uses tools such as images, lasers, and ultrasound to model the vehicle's motion and surroundings. The planning module combines the vehicle's surroundings and current motion information provided by the perception module to calculate a collision-free trajectory that conforms to vehicle dynamics. The control module controls the vehicle's movement based on the trajectory points output by the planning module.

[0003] For example, Chinese patent document CN109606354A discloses an automatic parking method based on hierarchical planning, which includes a perception module, an HMI display module, a path planning module, and a control and tracking module. The perception modules installed around the vehicle obtain information about obstacles in the surrounding environment, and calculate the size and type of the parking space and whether there are obstacles in the parking space. When the parking space size meets the requirements and there are no obstacles inside the parking space, an initial planning based on numerical optimization is performed. When the initial planning does not meet the parking requirements, an A* search planning and a secondary numerical optimization planning are performed based on the current vehicle posture, parking space information and environmental obstacle information. After successful planning, the trajectory control points are sent to the on-board controller, which controls the vehicle steering wheel, accelerator pedal and brake pedal to park the vehicle in the target parking space.

[0004] The planning module is a crucial component of automated parking systems. It outputs path and speed information, which are combined to form the trajectory. Direct trajectory planning and path-speed decoupled planning are two areas of active research. While the resulting path from trajectory planning is highly optimal, it carries a high computational complexity. Decoupled path-speed planning offers lower computational complexity but compromises the optimality of the resulting trajectory.

[0005] Path-velocity decoupled planning is widely used in engineering practice. Path planning can be categorized into sampling-based and optimization-based methods. Sampling-based methods often produce paths that are not smooth enough or do not fully meet vehicle dynamics requirements. Optimization-based methods mathematically model vehicle and environmental information, then minimize a cost function under appropriate constraints to determine the optimal path.

[0006] Existing path-velocity decoupled planning techniques require consideration of the vehicle's minimum turning radius constraint during path planning based on a mass-point model. Consequently, the final path is often determined using optimization-based methods. Because the vehicle's minimum turning radius cannot be reflected in the mass-point model, curvature constraints are converted. However, most existing solutions over-idealize the curvature problem, resulting in a path with a curvature radius smaller than the vehicle's minimum turning radius in some cases. This hinders stable vehicle control by downstream controller modules. Summary of the Invention

[0007] The present invention provides an automatic parking path planning method based on quadratic programming and parallel iteration, which can generate a collision-free automatic parking path planning that conforms to the vehicle kinematic model, thereby improving the safety, stability and comfort of the vehicle during the automatic parking process.

[0008] An automatic parking path planning method based on quadratic programming and parallel iteration includes the following steps:

[0009] (1) Initial path generation phase: In the feasible domain of the parking scenario, the hybrid A* path planning algorithm is used to generate an initial path that conforms to the vehicle kinematic model and does not collide with obstacles, entering the initial path decomposition phase;

[0010] (2) Initial path decomposition stage: The initial path is decomposed according to the speed direction switching points in the initial path to obtain a set of segmented initial paths, and then enter the segmented collision-free continuous path generation stage;

[0011] (3) Segmented collision-free continuous path generation stage: A quadratic programming problem for collision avoidance is constructed based on the segmented path elements in the segmented initial path set. Then, the segmented collision-free continuous path set corresponding to the segmented initial path set is obtained by dynamically adjusting the path point weights in parallel iterations, and then the curvature optimization stage is entered.

[0012] (4) Curvature optimization stage: A quadratic programming problem of collision avoidance and curvature optimization is constructed based on the elements in the set of segmented collision-free continuous paths. Then, a parking path that is collision-free and meets the curvature requirements is obtained by dynamically adjusting the weights of some objective functions in the optimization target in parallel iterations.

[0013] In step (1), the initial path that does not collide with the obstacle means that the vertex of the vertical projection of the vehicle outline on the ground has no overlapping part with the vertical projection area of the obstacle on the ground; wherein the obstacle refers to any object other than the parking space and lane to be parked.

[0014] In step (2), the sequence of speed direction switching points is expressed as:

[0015]

[0016] T={p1,Q,p n}

[0017] P={p1, ..., p n}, n∈N +

[0018] Among them, R min is the minimum turning radius of the vehicle, P is the discrete path point sequence generated by hybrid A*, and p k is the speed switching point, ∠(p k-1 p k+1 p k ) is the angle of the vertex corresponding to the kth point in the triangle formed by the k-1, k, and k+1 path points in the sequence of discrete path points generated by the hybrid A* algorithm; Q is the sequence of speed direction switching points excluding the start and end points of the sequential discrete path generated by the hybrid A* algorithm; T is the sequence of speed direction switching points including the start and end points of the sequential discrete path generated by the hybrid A* algorithm, and n is the number of discrete path points generated by the hybrid A* algorithm.

[0019] In step (2), the segmented initial path set is expressed as:

[0020] Pt={Pe1,...,Pe m-1}

[0021] Pe is the segmented initial path sequence, which is the path segment sequence between two speed direction switching points in sequence P, and m is the number of elements in sequence Q.

[0022] In step (3), the construction process of the quadratic programming problem for collision avoidance is as follows:

[0023] (3-1) Using the quintic polynomial f x (s) represents the relationship between the horizontal coordinate sequence x of the collision-free continuous path in the world coordinate system and the length sequence s of the collision-free continuous path. The analytical expression of the function is:

[0024] f x (s)=a5s 5 +a4s 4 +a3s 3 +a2s 2 +a1s 1 +a0

[0025] Among them, a0~a5 are the coefficients of the quintic polynomial; the elements in the sequence s represent the path length of the vehicle moving from the starting point of the segmented initial path to each other path point in the segmented initial path;

[0026] (3-2) Using the fifth-order polynomial f y (s) represents the relationship between the ordinate y of the collision-free continuous path in the world coordinate system and the length s of the collision-free continuous path. The analytical expression of the function is:

[0027] f y (s)=b5s 5 +b4s 4 +b3s 3 +b2s 2 +b1s 1 +b0

[0028] Among them, b0~b5 are the coefficients of the quintic polynomial;

[0029] (3-3) According to (3-1) and (3-2), the J1 part of the objective function of the quadratic function is constructed:

[0030]

[0031] Where t is the number of path points in the segmented initial path, the sequence c represents the weight coefficients of each path point in the segmented initial path, and the initial value is the unit vector [1...1] 1× t;x k ref ,y k re Indicates the horizontal and vertical coordinates of each path point in the segmented initial path in the world coordinate system; s k That is, the path length of the vehicle moving from the starting point of the segmented initial path to the kth path point of the segmented initial path;

[0032] (3-4) According to (3-1) and (3-2), the J2 part of the objective function of the quadratic function is constructed:

[0033]

[0034] Among them, f x (1) (s) represents f x (s) the first derivative of variable s, f y (1) (s) represents f y (s) the first derivative with respect to the variable s;

[0035] (3-5) According to (3-1) and (3-2), the J3 part of the objective function of the quadratic function is constructed:

[0036]

[0037] Among them, f x (2) (s) represents fx (s) the second derivative of variable s, f y (2) (s) represents f y (s) the second derivative with respect to the variable s;

[0038] (3-6) Based on (3-3) to (3-5), the quadratic programming objective function for collision avoidance is expressed as J:

[0039] J=w1*J1+w2*J2+w3*J3

[0040] Among them, w1, w2, w3 are the weight coefficients of each sub-objective function;

[0041] (3-7) The quadratic programming problem for collision avoidance is expressed as:

[0042] min J=w1*J1+w2*J2+w3*J3

[0043] st f x (s1) = x1 ref , f y (s1)=y1 ref

[0044] f x (s t )=x t ref , f y (s t )=y t ref

[0045] f x (s2) = x2 ref , f y (s2) = y2 ref

[0046] f x (s t-1 )=x t-1 ref , f y (s t-1 )=y t-1 ref .

[0047] In step (3), the method for dynamically adjusting the weight of the path points in the parallel iteration is specifically as follows: parallel means constructing and solving the quadratic programming problem for collision avoidance in parallel by multi-threaded parallel computing based on the segmented initial path elements in the segmented initial path set;

[0048] Iteration refers to establishing a single loop logic in each thread, performing collision detection in the loop, and exiting the loop if the parking path generated by the quadratic programming problem for collision avoidance does not cause a collision. If a collision occurs, the weight values of the collision path point and its nearby path points in sequence c are increased, and then the loop judgment is repeated until a collision-free parking path is found.

[0049] In step (3) and step (4), the collision detection method is: discrete sampling of the parking path generated by the quadratic planning is performed, and then the collision situation between the vehicle posture and the obstacle at each discrete path point is calculated.

[0050] In step (4), the quadratic programming problem of collision avoidance and curvature optimization is specifically:

[0051] (4-1) Using the quintic polynomial g x (d) represents the relationship between the horizontal coordinate x of the parking path in the world coordinate system and the length d of the parking path. The function expression is:

[0052] g x (d) = c5d 5 +c4d 4 +c3d 3 +c2d 2 +c1d 1 +c0

[0053] Where c0 to c5 are the coefficients of the quintic polynomial; the elements in the sequence d represent the path lengths from the starting point of the piecewise collision-free discrete path to each other discrete path point of the piecewise collision-free discrete path after the path of the piecewise collision-free continuous path is discretized into h path points;

[0054] (4-2) Using the quintic polynomial g y (d) represents the relationship between the ordinate y of the parking path in the world coordinate system and the length d of the parking path. The function expression is:

[0055] g y (d) = e5d 5 +e4d 4 +e3d 3 +e2d 2 +e1d 1 +e0

[0056] Among them, e0~e5 are the coefficients of the fifth-order polynomial;

[0057] (4-3) According to (4-1) and (4-2), the C1 part of the objective function of the quadratic function is constructed:

[0058]

[0059] Among them, f x (d i ), f y (d i ) represents the horizontal and vertical coordinates of each discrete path point in the piecewise collision-free discrete path in the world coordinate system, d i That is, the path length of the vehicle moving from the starting point of the piecewise collision-free discrete path to the i-th discrete path point of the piecewise collision-free discrete path;

[0060] (4-4) According to (4-1) and (4-2), the C2 part of the objective function of the quadratic function is constructed:

[0061]

[0062] Among them, g x (1) (d) represents g x (d) For the first derivative of d, g y (1) (d) represents g y (d) the first derivative with respect to d;

[0063] (4-5) According to (4-1) and (4-2), the C3 part of the objective function of the quadratic function is constructed:

[0064]

[0065] Among them, g x (2) (d) represents g x (d) For the second derivative of d, g y (2) (d) represents g y (d) the second derivative with respect to d;

[0066] (4-6) Based on (4-3) to (4-5), the quadratic programming objective function for collision avoidance and curvature optimization is expressed as C:

[0067] C=v1*C1+v2*C2+v3*C3

[0068] Among them, v1, v2, v3 are the weight coefficients of each sub-objective function;

[0069] (4-7) The quadratic programming problem of collision avoidance and curvature optimization can be expressed as:

[0070] min C=v1*C1+v2*C2+v3*C3

[0071] st g x (d1) = f x (d1), g y(d1) = f y (d1)

[0072] g x (d h )=f x (d h ), g y (d h )=f y (d h )

[0073] g x (d2) = f x (d2), g y (d2) = f y (d2)

[0074] g x (d h-1 )=f x (d h-1 ), g y (d h-1 )=f y (d h-1 ).

[0075] In step (4), the method for dynamically adjusting the weights of some objective functions in the optimization target in parallel iterations is specifically as follows:

[0076] Parallel means constructing and solving the quadratic programming problem of collision avoidance and curvature optimization in parallel based on the piecewise collision-free continuous path elements in the piecewise collision-free continuous path set through multi-threaded parallel computing;

[0077] Iteration means establishing a double loop logic in each thread, performing collision detection in the outer loop, and exiting the outer loop if the parking path generated by the quadratic planning problem for collision avoidance does not cause a collision. If a collision occurs, increase the value of v1. The iteration method is v1*e q-1 ,q≥1, q is the number of iterations of the outer loop; in the inner loop, the curvature of the parking path generated by the quadratic programming with collision avoidance and curvature optimization is judged. If the maximum curvature of the parking path exceeds the curvature limit, the values of v2 and v3 are increased, where the maximum curvature value is The iteration method is v2*2 p-1 ,v3*3 p-1 ,p≥1, p is the number of iterations of the inner loop; among them, the collision detection in the outer loop is performed after the inner loop ends, that is, collision detection is performed on the parking path whose curvature meets the requirements.

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

[0079] 1. The present invention converts the path curvature index in the automatic parking path planning into the iteration as a constraint, explicitly ensuring that the curvature of the final generated automatic parking path does not exceed the limit.

[0080] 2. The present invention decouples and iteratively considers the collision-free constraint and curvature compliance constraint in the path planning problem of automatic parking, thereby improving the robustness of the planning algorithm.

[0081] 3. The present invention decomposes the automatic parking path planning problem into a segmented path planning problem and combines it with parallel computing technology to greatly improve the operating efficiency of the planning algorithm and shorten the running time of the planning algorithm.

[0082] 4. The present invention utilizes quadratic programming in convex optimization to design an optimization problem including collision-free and curvature indicators, thereby improving the efficiency of the optimization solution. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 This is a flow chart of an automatic parking path planning method based on quadratic programming and parallel iteration according to the present invention;

[0084] Figure 2 A flow chart for obtaining collision-free continuous path segments in the present invention;

[0085] Figure 3 A flowchart of iteratively obtaining a path segment without collision and meeting curvature requirements in the present invention;

[0086] Figure 4 Schematic diagram of the initial path generated by hybridA* in the present invention;

[0087] Figure 5 Schematic diagram of a segmented initial path set after the initial path is decomposed in the present invention;

[0088] Figure 6 Schematic diagram of a segmented collision-free continuous path set in the present invention;

[0089] Figure 7 A schematic diagram of a parking path that is collision-free and meets curvature requirements in the present invention;

[0090] Figure 8 is the normalized curvature of the parking path that is collision-free and meets the curvature requirements in the present invention. DETAILED DESCRIPTION

[0091] The present invention will be described in further detail below with reference to the accompanying drawings and examples. It should be noted that the following examples are intended to facilitate understanding of the present invention and do not have any limiting effect on the present invention.

[0092] like Figure 1As shown, an automatic parking path planning method based on quadratic programming and parallel iteration includes the following steps:

[0093] Step 1: Generate an initial path that conforms to the vehicle kinematic model and does not collide with obstacles using the hybrid A* path planning algorithm. The initial path generated by hybrid A* is as follows: Figure 4 shown.

[0094] Step 2: Decompose the initial path according to the vehicle running direction switching points in the initial path.

[0095]

[0096] T={p1,Q,p n}

[0097] P={p1, ..., p n}, n∈N +

[0098] R min is the minimum turning radius of the vehicle. In order to eliminate the influence of calculation error on the algorithm, R min Multiply by a coefficient of 0.8. P is the discrete path point sequence generated by hybrid A*, p k is the speed switching point, ∠(p k-1 p k+1 p k ) is the angle of the vertex corresponding to the kth point in the triangle formed by the k-1, k, and k+1 path points in the sequence of discrete path points generated by the hybrid A* algorithm. Q is the sequence of speed direction switching points excluding the start and end points of the sequential discrete path generated by the hybrid A* algorithm. T is the sequence of speed direction switching points including the start and end points of the sequential discrete path generated by the hybrid A* algorithm. n is the number of discrete path points generated by the hybrid A* algorithm.

[0099] The path segment Pe between two speed direction switching points in the sequence P is the segmented initial path. The segmented initial path set Pt is as follows: Figure 5 As shown. The segmented initial path set Pt is expressed as:

[0100] Pt={Pe1,...,Pe m-1}

[0101] m is the number of elements in the sequence Q.

[0102] Step 3: Construct a quadratic programming problem for collision avoidance based on the segmented path elements in the segmented initial path set, and then obtain the segmented collision-free continuous path set corresponding to each original path segment set by dynamically adjusting the path point weights in parallel iterations. The specific process is as follows: Figure 2 As shown, the final set of segmented collision-free continuous paths is as follows Figure 6 shown.

[0103] Step 3-1, extract the quintic polynomial f x (s) coefficients to form F x k (X) = A k X in the form of:

[0104] f x (s)=a s s 5 +a4s 4 +a3s 3 +a2s 2 +a1s 1 +a0

[0105] Bring in s k ,

[0106] f x (s k )=a5s k 5 +a4s k 4 +a3s k 3 +a2s k 2 +a1s k 1 +a0

[0107] F x k (X) = AX

[0108] =[s k 5 s k 4 s k 3 s k 2 s k 1 1][a5 a4 a3 a2 a1 a0] T

[0109] Extract the quintic polynomial f y (s) coefficients to form F y k (X) = B k X in the form of:

[0110] f y (s)=b5s 5 +b4s 4+b3s 3 +b2s 2 +b1s 1 +b0

[0111] Bring in s k ,

[0112] f y (s k )=b5s k 5 +b4s k 4 +b3s k 3 +b2s k 2 +b1s k 1 +b0

[0113] F y k (X) = BX

[0114] =[s k 5 s k 4 s k 3 s k 2 s k 1 1][b5 b4 b3 b2 b1 b0] T

[0115] Among them, the fifth-order polynomial f x (s) is the analytical expression of the function that represents the horizontal coordinate sequence x of the collision-free continuous path in the world coordinate system and the length sequence s of the collision-free continuous path. The fifth-order polynomial f y (s) is the function expression that represents the vertical coordinate sequence y of the collision-free continuous path in the world coordinate system and the length sequence s of the collision-free continuous path. a0~a5 are the fifth-order polynomials f x The coefficients of (s), b0~b5 are the fifth-order polynomial f y The elements in the sequence s represent the path lengths of the vehicle from the starting point of the segmented initial path to the other path points in the segmented initial path, s k That is, the path length of the vehicle moving from the starting point of the segmented initial path to the kth path point of the segmented initial path.

[0116] Step 3-2, construct the J1 part of the objective function of the quadratic function:

[0117]

[0118] Where t is the number of path points in the segmented initial path, the sequence c represents the weight coefficients of each path point in the segmented initial path, and the initial value is the unit vector [1...1] 1×t .x k ref ,y k ref Indicates the horizontal and vertical coordinates of each path point in the segmented initial path in the world coordinate system. k That is, the path length of the vehicle moving from the starting point of the segmented initial path to the kth path point of the segmented initial path.

[0119] Step 3-3, construct the J2 part of the objective function of the quadratic function:

[0120]

[0121] where f x (1) (s) represents f x (s) the first derivative of variable s, f y (1) (s) represents f y (s) The first derivative of variable s. (1) k =B (1) k =[5*s k 4 4*s k 3 3*s k 2 2*s k 1 1 0]

[0122] Step 3-4, construct the J3 part of the objective function of the quadratic function:

[0123]

[0124] where f x (2) (s) represents f x (s) the second derivative of variable s, f y (2) (s) represents f y (s) The second derivative of variable s. (2) k =B (2) k =[20*s k 3 12*s k 12 6*s k1 2 0 0]

[0125] Steps 3-5, construct the quadratic programming objective function for collision avoidance expressed as J:

[0126] J=w1*J1+w2*J2+w3*J3

[0127] Among them, w1=3, w2=1, w3=1 are the weight coefficients of each sub-objective function.

[0128] Steps 3-6, the quadratic programming problem for collision avoidance is expressed as:

[0129] min J=w1*J1+w2*J2+w3*J3

[0130] st A1X=x1 ref , B1X=y1 ref

[0131] A t X=x t ref , B t X=y t ref

[0132] A2X=x2 ref , B2X=y2 ref

[0133] A t-1 X=x t-1 ref , B t-1 X=y t-1 ref

[0134] In steps 3-7, the method for dynamically adjusting pathpoint weights during parallel iterations is as follows: Parallelism refers to constructing and solving a collision avoidance quadratic programming problem in parallel using multi-threaded parallel computing based on the segmented initial path elements in the segmented initial path set. Iteration refers to establishing a single loop logic within each thread, which performs collision detection. If the parking path generated by the collision avoidance quadratic programming problem is collision-free, the loop exits. If a collision occurs, the weights of the collision pathpoint and its nearby pathpoints in sequence c are increased, and the loop is repeated until a collision-free parking path is found. Obstacles are any objects other than the parking space or lane. The specific collision detection method is to discretely sample the parking path generated by the quadratic programming and then calculate the collision status of the vehicle's posture with obstacles at each discrete pathpoint. The specific collision detection method is to determine a collision-free situation only if the convex hull of the vehicle's perpendicular projection to the ground at a certain point does not overlap with the convex hulls of all obstacles' perpendicular projections to the ground. Otherwise, the vehicle is considered to have collided with an obstacle at that pathpoint.

[0135] Step 4: Based on the elements in the set of segmented collision-free continuous paths, a quadratic programming problem for collision avoidance and curvature optimization is constructed. Then, a collision-free parking path that meets curvature requirements is obtained by dynamically adjusting the optimization target weights in parallel iterations. The specific process is as follows: Figure 3 As shown in the figure, the final parking path without collision and meeting the curvature requirements is as follows Figure 7 shown.

[0136] Step 4-1, extract the quintic polynomial g x (d) coefficients to form G x i (X)=H i X in the form of:

[0137] g x (d) = c5d 5 +c4d 4 +c3d 3 +c2d 2 +c1d 1 +c0

[0138] Bring in d i ,

[0139] G x i (X)=H i X=[d i 5 d i 4 d i 3 d i 2 d i1 1][c5 c4 c3 c2 c1 c0] T

[0140] Extract the quintic polynomial g y (d) coefficients to form G y i (X) = E i X in the form of:

[0141] g y (s)=e5d 5 +e4d 4 +e3d 3 +e2d 2 +e1d 1 +e0

[0142] Bring in d i ,

[0143] G y i (X) = E i X=[d i 5 d i 4 d i 3 d i 2 d i 1 1][e5 e4 e3 e2 e1 e0] T

[0144] Among them, the fifth-order polynomial g x (d) The analytical expression of the function representing the horizontal coordinate x of the parking path in the world coordinate system and the parking path length d, where c0 to c5 are quintic polynomials g x (d) Coefficients of the quintic polynomial g y (d) The analytical expression of the function representing the ordinate y of the parking path in the world coordinate system and the length d of the parking path, where e0 to e5 are quintic polynomials g y The elements in the sequence d represent the path lengths from the starting point of the piecewise collision-free discrete path to the other discrete path points of the piecewise collision-free discrete path after the path of the piecewise collision-free continuous path is discretized into h path points. i That is, the path length of the vehicle moving from the starting point of the piecewise collision-free discrete path to the i-th discrete path point of the piecewise collision-free discrete path.

[0145] Step 4-2, construct the C1 part of the objective function of the quadratic function:

[0146]

[0147] f x (d i ), f y (d i ) represents the horizontal and vertical coordinates of each discrete path point in the piecewise collision-free discrete path in the world coordinate system. i That is, the path length of the vehicle moving from the starting point of the piecewise collision-free discrete path to the i-th discrete path point of the piecewise collision-free discrete path.

[0148] Step 4-3, construct the C2 part of the objective function of the quadratic function:

[0149]

[0150] where g x (1) (d) represents g x (d) For the first derivative of d, g y (1) (d) represents g y (d) The first derivative with respect to d.

[0151] H (1) i =E (1) i =[5*d i 4 4*d i 3 3*d i 2 2*d i 1 1 0]

[0152] Step 4-4, construct the C3 part of the objective function of the quadratic function:

[0153]

[0154] where g x (2) (d) represents g x (d) For the second derivative of d, g y (2) (d) represents g y (d) The second derivative with respect to d.

[0155] H (2) i =E (2) i =[20*d i 3 12*d i 2 6*di 1 2 0 0]

[0156] Steps 4-5, construct the quadratic programming objective function for collision avoidance and curvature optimization, expressed as C:

[0157] C=v1*C1+v2*C2+v3*C3

[0158] Among them, v1=1, v2=2, v3=3 are the weight coefficients of each sub-objective function.

[0159] Steps 4-6, the quadratic programming problem for collision avoidance and curvature optimization is expressed as:

[0160] min C=v1*C1+v2*C2+v3*C3

[0161] st H1X=f x (d1), E1X=f y (d1)

[0162] H h X=f x (d h ), E h X=f y (d h )

[0163] H2X=f x (d2), E2X=f y (d2)

[0164] H h-1 X=f x (d h-1 ), E h-1 X=f y (d h-1 )

[0165] Steps 4-7, the method for dynamically adjusting the optimization target weight in parallel iteration is expressed as follows: Parallel means constructing and solving the quadratic programming problem of collision avoidance and curvature optimization in parallel based on the segmented collision-free continuous path elements in the segmented collision-free continuous path set through multi-threaded parallel computing; Iteration means establishing a double loop logic in each thread, performing collision detection in the outer loop, and exiting the outer loop if the parking path generated by the quadratic programming problem of collision avoidance does not cause a collision. If a collision occurs, the value of v1 is increased, and the iteration method is v1*e q-1 ,q≥1, q is the number of iterations of the outer loop. In the inner loop, the curvature of the parking path generated by the collision avoidance and curvature optimization quadratic programming is judged. If the maximum curvature of the parking path exceeds the curvature limit, the values of v2 and v3 are increased, where the maximum curvature value is The iteration method is v2*2 p-1 ,v3*3 p-1 ,p≥1, p is the number of iterations of the inner loop. Among them, the collision detection in the outer loop is performed after the inner loop ends, that is, the collision detection is performed on the parking path whose curvature meets the requirements. The normalized curvature of the parking path without collision and meeting the curvature requirements is as follows Figure 8 shown.

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

Claims

1. An automatic parking path planning method based on quadratic programming and parallel iteration, characterized in that: The following steps are involved: (1) Initial path generation phase: In the feasible domain of the parking scenario, the hybrid A* path planning algorithm is used to generate an initial path that conforms to the vehicle kinematic model and does not collide with obstacles, entering the initial path decomposition phase; (2) Initial path decomposition stage: The initial path is decomposed according to the speed direction switching points in the initial path to obtain a set of segmented initial paths, and then enter the segmented collision-free continuous path generation stage; (3) Segmented collision-free continuous path generation stage: A quadratic programming problem for collision avoidance is constructed based on the segmented path elements in the segmented initial path set. Then, the segmented collision-free continuous path set corresponding to the segmented initial path set is obtained by dynamically adjusting the path point weights in parallel iterations, and then the curvature optimization stage is entered. (4) Curvature optimization stage: A quadratic programming problem of collision avoidance and curvature optimization is constructed based on the elements in the set of segmented collision-free continuous paths. Then, a parking path that is collision-free and meets the curvature requirements is obtained by dynamically adjusting the weights of some objective functions in the optimization target in parallel iterations.

2. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (1), the initial path that does not collide with the obstacle means that the vertex of the vertical projection of the vehicle outline on the ground has no overlapping part with the vertical projection area of the obstacle on the ground; wherein the obstacle refers to any object other than the parking space and lane to be parked.

3. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (2), the sequence of speed direction switching points is expressed as: T={p l ,Q,p n } P={p l ,·,p n },n∈N + Among them, R min is the minimum turning radius of the vehicle, P is the discrete path point sequence generated by hybrid A*, and p k is the speed switching point, ∠(p k-1 p k+1 p k ) is the angle of the vertex corresponding to the kth point in the triangle formed by the k-1, k, and k+1 path points in the sequence of discrete path points generated by the hybrid A* algorithm; Q is the sequence of speed direction switching points excluding the start and end points of the sequential discrete path generated by the hybrid A* algorithm; T is the sequence of speed direction switching points including the start and end points of the sequential discrete path generated by the hybrid A* algorithm, and n is the number of discrete path points generated by the hybrid A* algorithm.

4. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (2), the segmented initial path set is expressed as: Pt={For1,...,For m-1 } Pe is the segmented initial path sequence, which is the path segment sequence between two speed direction switching points in sequence P, and m is the number of elements in sequence Q.

5. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (3), the construction process of the quadratic programming problem for collision avoidance is as follows: (3-1) Using the quintic polynomial f x (s) represents the relationship between the horizontal coordinate sequence x of the collision-free continuous path in the world coordinate system and the length sequence s of the collision-free continuous path. The analytical expression of the function is: f x (s)=a5s 5 +a4s 4 +a3s 3 +a2s 2 +a l s 1 +a0 Among them, a0~a5 are the coefficients of the quintic polynomial; the elements in the sequence s represent the path length of the vehicle moving from the starting point of the segmented initial path to each other path point in the segmented initial path; (3-2) Using the fifth-order polynomial f y (s) represents the relationship between the ordinate y of the collision-free continuous path in the world coordinate system and the length sequence s of the collision-free continuous path. The analytical expression of the function is: f y (s)=b5s 5 +b4s 4 +b3s 3 +b2s 2 +b1s 1 +b0 Among them, b0~b5 are the coefficients of the quintic polynomial; (3-3) According to (3-1) and (3-2), the J1 part of the objective function of the quadratic function is constructed: Where t is the number of path points in the segmented initial path, the sequence c represents the weight coefficients of each path point in the segmented initial path, and the initial value is the unit vector [1…1] 1×t ;x k ref ,y k ref Indicates the horizontal and vertical coordinates of each path point in the segmented initial path in the world coordinate system; s k That is, the path length of the vehicle moving from the starting point of the segmented initial path to the kth path point of the segmented initial path; (3-4) According to (3-1) and (3-2), the J2 part of the objective function of the quadratic function is constructed: Among them, f x (1) (s) represents f x (s) the first derivative of variable s, f y (1) (s) represents f y (s) the first derivative with respect to the variable s; (3-5) According to (3-1) and (3-2), the J3 part of the objective function of the quadratic function is constructed: Among them, f x (2) (s) represents f x (s) the second derivative of variable s, f y (2) (s) represents f y (s) the second derivative with respect to the variable s; (3-6) Based on (3-3) to (3-5), the quadratic programming objective function for collision avoidance is expressed as J: J=w1*J1+w2*J2+w3*J3 Among them, w1, w2, w3 are the weight coefficients of each sub-objective function; (3-7) The quadratic programming problem for collision avoidance is expressed as: min J=w1*J1+w2*J2+w3*J3 s.t. f x (s1)=x1 ref ,f y (s1)=y1 ref f x (s t )=x t ref ,f y (s t )=y t ref <h2 style=";text-align:left;direction:ltr">f<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> (s2) = x2<h2 style=";text-align:left;direction:ltr"> ref <h2 style=";text-align:left;direction:ltr"> ,f<h2 style=";text-align:left;direction:ltr"> y <h2 style=";text-align:left;direction:ltr"> (s2)=y2<h2 style=";text-align:left;direction:ltr"> ref f x (s t-1 )=x t-1 ref ,f y (s t-1 )=y t-1 ref 。 6. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 5, characterized in that: In step (3), the method for dynamically adjusting the weights of path points in parallel iteration is specifically as follows: parallel means constructing and solving the quadratic programming problem for collision avoidance in parallel by multi-threaded parallel computing based on the segmented initial path elements in the segmented initial path set; iteration means establishing a single loop logic in each thread, performing collision detection in the loop, and exiting the loop if the parking path generated by the quadratic programming problem for collision avoidance does not cause a collision; if a collision occurs, increasing the weight values of the collision path point and its nearby path points in sequence c, and then re-performing the loop judgment until a collision-free parking path is found.

7. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (3) and step (4), the collision detection method is: discrete sampling of the parking path generated by the quadratic planning is performed, and then the collision situation between the vehicle posture and the obstacle at each discrete path point is calculated.

8. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 1, characterized in that: In step (4), the quadratic programming problem of collision avoidance and curvature optimization is specifically: (4-1) Using the quintic polynomial g x (d) represents the relationship between the horizontal coordinate x of the parking path in the world coordinate system and the length d of the parking path. The function expression is: g x (d)=c5d 5 +c4d 4 +c3d 3 +c2d 2 +c1d 1 +c o Where c0 to c5 are the coefficients of the quintic polynomial; the elements in the sequence d represent the path lengths from the starting point of the piecewise collision-free discrete path to each other discrete path point of the piecewise collision-free discrete path after the path of the piecewise collision-free continuous path is discretized into h path points; (4-2) Using the quintic polynomial g y (d) represents the relationship between the ordinate y of the parking path in the world coordinate system and the length d of the parking path. The function expression is: gy (d)=e5d 5 +e4d 4 +e3d 3 +e2d 2 +e1d 1 +e0 Among them, e0~e5 are the coefficients of the fifth-order polynomial; (4-3) According to (4-1) and (4-2), the C1 part of the objective function of the quadratic function is constructed: Among them, f x (d i ),f y (d i ) represents the horizontal and vertical coordinates of each discrete path point in the piecewise collision-free discrete path in the world coordinate system, d i That is, the path length of the vehicle moving from the starting point of the piecewise collision-free discrete path to the i-th discrete path point of the piecewise collision-free discrete path; (4-4) According to (4-1) and (4-2), the C2 part of the objective function of the quadratic function is constructed: Among them, g x (1) (d) represents g x (d) For the first derivative of d, g y (1) (d) represents g y (d) the first derivative with respect to d; (4-5) According to (4-1) and (4-2), the C3 part of the objective function of the quadratic function is constructed: Among them, g x (2) (d) represents g x (d) For the second derivative of d, g y (2) (d) represents g y (d) the second derivative with respect to d; (4-6) Based on (4-3) to (4-5), the quadratic programming objective function for collision avoidance and curvature optimization is expressed as C: C=v1*C1+v2*C2+v3*C3 Among them, v1, v2, v3 are the weight coefficients of each sub-objective function; (4-7) The quadratic programming problem of collision avoidance and curvature optimization can be expressed as: min C=v1*C1+v2*C2+v3*C3 s.t g x (d1)=f x (d1),g y (d1)=f y (d1) g x (d h )=f x (d h ),g y (d h )=f y (d h ) g x (d2)=f x (d2),g y (d2)=f y (d2) g x (d h-1 )=f x (d h-1 ),g y (d h-1 )=f y (d h-1 )。 9. The automatic parking path planning method based on quadratic programming and parallel iteration according to claim 8, characterized in that: In step (4), the method for dynamically adjusting the weights of some objective functions in the optimization target in parallel iterations is specifically as follows: Parallel means constructing and solving the quadratic programming problem of collision avoidance and curvature optimization in parallel based on the piecewise collision-free continuous path elements in the piecewise collision-free continuous path set through multi-threaded parallel computing; Iteration means establishing a double loop logic in each thread, performing collision detection in the outer loop, and exiting the outer loop if the parking path generated by the quadratic planning problem for collision avoidance does not cause a collision. If a collision occurs, increase the value of v1. The iteration method is v1*e q-1 ,q≥1, q is the number of iterations of the outer loop; in the inner loop, the curvature of the parking path generated by the quadratic programming with collision avoidance and curvature optimization is judged. If the maximum curvature of the parking path exceeds the curvature limit, the values of v2 and v3 are increased, where the maximum curvature value is The iteration method is v2*2 p-1 ,v3*3 p-1 ,p≥1, p is the number of iterations of the inner loop; among them, the collision detection in the outer loop is performed after the inner loop ends, that is, collision detection is performed on the parking path whose curvature meets the requirements.

Images