Transverse obstacle avoidance planning method based on expansion coefficient dual transformation

Abstract

A transverse obstacle avoidance planning method based on expansion coefficient dual transformation comprises the following steps that S1, a vehicle polygon convex set and an obstacle polygon convex set are constructed based on surrounding obstacle polygon envelope; s2, combining the motion model of the vehicle and the polygonal convex set constraint of the obstacle to construct an optimal control problem; and S3, solving an optimal state sequence and a control sequence according to the optimal control problem in the step S2, and calculating a real-time path under the Cartesian coordinate system. According to the invention, through dual transformation constraint of the collision coefficient, a constraint expression form capable of being used for accurate obstacle avoidance is effectively provided, and an optimal control problem is established to solve an optimal solution.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent driving, and in particular relates to a lateral obstacle avoidance planning method based on dual transformation of expansion coefficients. Background Art

[0002] The purpose of lateral planning is to generate a vehicle path that avoids static and some dynamic obstacles and tracks the vehicle's global reference line.

[0003] The main technical difficulty of lateral planning lies in the modeling of obstacle avoidance conditions. The existing technical route is mainly edited based on the Frenet coordinate system, with the reference line as the coordinate axis, to establish boundary conditions related to the lateral distance l between the obstacle and the reference line, and calculate the optimal path.

[0004] The advantage of the existing technology is that the modeling is relatively simple and intuitive, but the disadvantage is that it simplifies the non-smooth characteristics of the path planning problem, thereby losing the collision avoidance accuracy. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the shortcomings of the prior art and provide a lateral obstacle avoidance planning method based on the dual transformation of the expansion coefficient, which can realize the precise separation of the convex set of the ego-vehicle and the convex set of the obstacle. Through the dual transformation constraint of the collision coefficient, an effective constraint expression form that can be used for precise obstacle avoidance is provided, and an optimal control problem is established to solve the optimal solution.

[0006] The present invention provides a lateral obstacle avoidance planning method based on expansion coefficient dual transformation, comprising the following steps:

[0007] Step S1. constructing a convex set of vehicle polygons and a convex set of obstacle polygons based on the polygonal envelope of the surrounding obstacles;

[0008] Step S2. Combining the vehicle's motion model with the polygonal convex set constraints of the obstacle, construct an optimal control problem;

[0009] Step S3. Solve the optimal state sequence and control sequence according to the optimal control problem in step S2 to calculate the real-time path in the Cartesian coordinate system.

[0010] As a further technical solution of the present invention, the specific process of step S1 is as follows:

[0011] Step S11. Calculate the kth polygonal convex set constraint based on the polygonal envelope of the obstacle

[0012]

[0013] Step S12: Repeat step S11 for obstacles within the range.

[0014] Furthermore, the specific process of step S2 is:

[0015] Step 21. Construct the state space equation based on the vehicle's motion model:

[0016]

[0017] Step S22. Based on the vehicle state and the polygonal envelope of the vehicle, the ego-vehicle convex set constraint is constructed as

[0018] A s T i (PP i )≤α k b s ;

[0019] Step S23. Based on the polygon convex set constraint and the self-car convex set constraint, the expansion coefficient dual transformation constraint is constructed as follows:

[0020]

[0021] Step S24. Construct the optimal control cost function based on the vehicle state and control amount:

[0022]

[0023] Step S25. Construct state constraints based on the vehicle's motion model constraints:

[0024] v l ≤v i ≤v u

[0025] κ l ≤κ i ≤κ u

[0026] dκ l ≤dκ i ≤dκ u ;

[0027] Step S26. Based on steps S21 to S25, construct the optimal control problem as

[0028]

[0029] v l ≤v i ≤v u

[0030] κ l ≤κ i ≤κ u

[0031] dκ ; ≤dκi ≤dκ u

[0032]

[0033]

[0034] Further, in step S22, the self-car convex set constraint is constructed with the bounding box as

[0035]

[0036] The advantages of the present invention are:

[0037] Achieve accurate separation of the convex set of the ego-vehicle and the convex set of obstacles.

[0038] The variables involved in the dual constraints are all state and intermediate algebraic variables, which will not increase the number of variables to be solved, thereby not adding too much computational burden while solving the optimal solution.

[0039] The dual constraints are continuously differentiable in space and do not cause numerical problems in the solution process. DETAILED DESCRIPTION

[0040] This embodiment provides a lateral obstacle avoidance planning method based on expansion coefficient dual transformation of the present invention:

[0041] S1, construct the ego-vehicle polygon convex set and the obstacle polygon convex set based on the surrounding obstacle polygon envelope; Step 1.1 calculates the kth polygon convex set constraint based on the obstacle polygon envelope

[0042] Step 1.2 Repeat step 1.1 for all obstacles within range.

[0043] S2, combining the vehicle's motion model with the polygonal convex set constraints of obstacles to construct the optimal control problem

[0044] Step 2.1 Construct the state space equations based on the vehicle's motion model

[0045]

[0046] Step 2.2: Construct the ego-vehicle convex set constraint based on the vehicle state and the vehicle's polygonal envelope.

[0047] A s T i (PP i )≤α k b s

[0048] Take the bounding box as an example, where:

[0049]

[0050] Step 2.3 Construct the dual transformation constraint of the expansion coefficient based on the polygon convex set constraint and the self-car convex set constraint

[0051]

[0052] Step 2.4: Construct the optimal control cost function based on vehicle state and control quantity

[0053]

[0054] Step 2.5: Construct state constraints based on the vehicle's motion model constraints

[0055] v l ≤v i ≤v u

[0056] κ l ≤κ i ≤κ u

[0057] dκ l ≤dκ i ≤dκ u .

[0058] Step 2.6: Based on steps 2.1 to 2.5, construct the optimal control problem

[0059]

[0060] v l ≤v i ≤v u

[0061] κ l ≤κ i ≤κ u

[0062] dκ l ≤dκ i ≤dκ u

[0063]

[0064] S3. Solve the optimal state sequence and control sequence based on the S2 optimal control problem to calculate the real-time path in the Cartesian coordinate system

[0065] Through the dual transformation constraint of the collision coefficient, an effective constraint expression form that can be used for accurate obstacle avoidance is provided, and an optimal control problem is established to solve the optimal solution.

[0066] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above specific embodiments. The above specific embodiments and the description in the specification are only for further illustrating the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention to be protected. The scope of the present invention to be protected is defined by the claims and their equivalents.

Claims

1. A lateral obstacle avoidance planning method based on expansion coefficient dual transformation, characterized in that: The method comprises the following steps: step S1. constructing a polygonal convex set of the vehicle and a polygonal convex set of obstacles based on the polygonal envelope of the surrounding obstacles; step S2. constructing an optimal control problem by combining the motion model of the vehicle and the polygonal convex set constraints of the obstacles; step S3. solving the optimal state sequence and control sequence according to the optimal control problem of step S2 to calculate the real-time path in the Cartesian coordinate system.

2. The lateral obstacle avoidance planning method based on expansion coefficient dual transformation according to claim 1, characterized in that: The specific process of step S1 is: Step S11. Calculate the kth polygonal convex set constraint based on the polygonal envelope of the obstacle Among them, Ak represents the coefficient matrix of the polygon equation, P represents the geometric center point of the polygon, αk represents the polygon expansion coefficient, bk represents the constant term of the polygon equation, and (hat)bk represents the offset. Step S12: Repeat step S11 for obstacles within the range.

3. The lateral obstacle avoidance planning method based on expansion coefficient dual transformation according to claim 1 is characterized in that: The specific process of step S2 is: Step 21. Construct the state space equation based on the vehicle's motion model: Where X represents the set of state quantities, U represents the set of control quantities, x, y represent the coordinates of the vehicle in the world coordinate system, θ represents the vehicle heading angle, κ represents the path curvature, v represents the vehicle speed, and dκ represents the rate of change of the path curvature. Step S22. Based on the vehicle state and the polygonal envelope of the vehicle, the convex set constraint of the vehicle is constructed as A8T i (PP i )≤α k b8; Step S23. Based on the polygon convex set constraint and the self-car convex set constraint, the expansion coefficient dual transformation constraint is constructed as follows: Where T represents the Jacobian matrix of coordinate transformation, λ and μ represent Lagrange multipliers; Step S24. Construct the optimal control cost function based on the vehicle state and control amount: Step S25. Construct state constraints based on the vehicle's motion model constraints: in l ≤in i ≤in u k l ≤κ i ≤κ u dk l ≤dk i ≤dk u ; Where vl and vu represent the lower and upper bounds of the vehicle speed, respectively; Kl and Ku represent the lower and upper bounds of the curvature, respectively; dK| and dKu represent the lower and upper bounds of the curvature change rate, respectively; Step S26. Based on Steps S21 to S25, the optimal control problem is constructed as in l ≤in i ≤in u k l ≤κ i ≤κ u dk i ≤dκ i ≤dκ u 4. The lateral obstacle avoidance planning method based on expansion coefficient dual transformation according to claim 3 is characterized in that: In step S22, the self-vehicle convex set constraint is constructed by using the bounding box as follows: Among them, 1f represents the distance from the center of mass of the vehicle to the front overhang of the vehicle, 1r represents the distance from the center of mass of the vehicle to the rear of the vehicle, and w represents the width of the vehicle.

Images