阅读说明：本技术 一种基于贝塞尔曲线的转向路段路径规划方法 (Bezier curve-based steering road section path planning method ) 是由 秦兆博 陈鑫 边有钢 秦晓辉 胡满江 徐彪 秦洪懋 谢国涛 王晓伟 丁荣军 于 2021-07-01 设计创作，主要内容包括：本发明实施例提供一种基于贝塞尔曲线的转向路段路径规划方法,以三条贝塞尔曲线规划所述车辆的转向路径,第一条贝塞尔曲线位于所述第一条道路,与第一条贝塞尔曲线连接的第二条贝塞尔曲线位于所述第一条道路和所述第二条道路的重叠区域,与所述第二条贝塞尔曲线连接的第三条贝塞尔曲线位于所述第二条道路。采用本发明实施例提供的方法,设置路径规划的起始状态和终止状态,建立路径规划的模型和限制条件,能够保证贝塞尔曲线的避障要求,满足路径规划的需要。(The embodiment of the invention provides a method for planning a path of a turning road section based on Bezier curves, wherein three Bezier curves are used for planning the turning path of a vehicle, a first Bezier curve is positioned on a first road, a second Bezier curve connected with the first Bezier curve is positioned in an overlapping area of the first road and the second road, and a third Bezier curve connected with the second Bezier curve is positioned on the second road. By adopting the method provided by the embodiment of the invention, the initial state and the termination state of the path planning are set, and the model and the limiting conditions of the path planning are established, so that the obstacle avoidance requirement of the Bezier curve can be ensured, and the requirement of the path planning can be met.)

1. A method for planning a path of a steered section based on bezier curves, wherein the steered section comprises a first road before a vehicle is steered and a second road after the vehicle is steered, the steered path of the vehicle is planned by three bezier curves, a first bezier curve is positioned on the first road, a second bezier curve connected with the first bezier curve is positioned in an overlapping region of the first road and the second road, and a third bezier curve connected with the second bezier curve is positioned on the second road, the method comprising:

step 1, determining an initial state and a termination state of path planning; wherein the starting state comprises a longitudinal and lateral coordinate of a starting position of the vehicle and a yaw angle of the vehicle at the starting position, and the ending state comprises a longitudinal and lateral coordinate of an ending position of the vehicle and a yaw angle of the vehicle at the ending position;

step 2, establishing a first quadrilateral model based on the starting position and the ending position of the vehicle and the boundary of the first road and the second road, shrinking the edge of the first quadrilateral model, taking a shrunk second quadrilateral model as a corridor model, including a first corridor in the first road and a second corridor in the second road, wherein the first corridor and the second corridor are overlapped at a turning position, and the shrinking width is R_sWherein

Wherein L is_wIs the width of the vehicle, L_xIs the wheel base of the vehicle, L_RIs the rear overhang length of the vehicle;

the expressions of the four edges of the first corridor are as follows:

a₁₁x+b₁₁y+c₁₁＝0，a₁₂x+b₁₂y+c₁₂＝0，a₁₃x+b₁₃y+c₁₃＝0，a₁₄x+b₁₄y+c₁₄＝0；

the expressions of the four sides of the second corridor are as follows:

a₂₁x+b₂₁y+c₂₁＝0，a₂₂x+b₂₂y+c₂₂＝0，a₂₃x+b₂₃y+c₂₃＝0，a₂₄x+b₂₄y+c₂₄＝0；

based on the start and end positions of the vehicle, the boundaries of the first and second roads, and the value of the contraction width Rs, the coefficient a may be determined₁₁、b₁₁、c₁₁、a₁₂、b₁₂、c₁₂、a₁₃、b₁₃、c₁₃、a₁₄、b_i4、c₁₄And coefficient a₂₁、b₂₁、c₂₁、a₂₂、b₂₂、c₂₂、a₂₃、b₂₃、c₂₃、a₂₄、b₂₄、c₂₄A value of (d);

the corridor model is marked as T₁And T₂：

Step 3, establishing an optimization model, wherein the optimization model comprises the following steps: decision variables, objective functions and constraint conditions; the decision variable is a control point coordinate of the Bezier curve, the Bezier curve is a quintic Bezier curve, and the control point coordinate comprises P_ij(x_i，j，y_i，j) 1,2,3, j 0, 1.. 5, wherein P is_ij(ii) the j +1 th control point representing the ith curve, (x)_i，j，y_i，j) Coordinates of the j +1 th control point representing the ith curve; the target function is the total length of the three Bezier curves; the constraint conditions comprise initial position constraint and final position constraint, position continuous constraint and yaw angle continuous constraint at the curve connection part, and obstacle avoidance constraint; calculating coordinate values of a control point when the total length of the three Bezier curves is shortest based on the decision variables, the objective function and the constraint conditions, and obtaining the three Bezier curves according to the relation between the coordinate of the control point and the Bezier curves, wherein the relation between the coordinate of the control point and the Bezier curves is as follows:

wherein s ∈ (0,1) is a continuous variable;

and 4, taking the three Bezier curve tracks obtained in the step 3 as the steering path of the vehicle.

2. The method of claim 1, wherein the objective function is expressed as:

wherein the content of the first and second substances,

the matrix M satisfies:

the matrix Q satisfies:

the starting position constraint and the ending position constraint are as follows:

wherein x is_iniAnd y_iniRespectively the abscissa, x, of the starting position of the vehicle_terAnd y_terRespectively, the horizontal and vertical coordinates of the end position of the vehicle;

the position of the curve connection is continuously constrained as follows:

P_i，5＝P_i+1，0，i＝1，2

the continuous constraint of the yaw angle at the curve junction is as follows:

P_i，5-P_i，4＝P_i+1，1-P_i+1，0，i＝1，2

the obstacle avoidance constraint includes:

a first obstacle avoidance constraint that limits the vehicle from traveling to an overlapping area of the first corridor and the second corridor based on the first Bezier curve and from colliding with a boundary of the first corridor:

a second obstacle avoidance constraint that limits the vehicle to pass through an overlapping region of the first corridor and the second corridor based on the second Bezier curve and not collide with a boundary of the two corridors:

a third obstacle avoidance constraint that limits the vehicle to traverse the second corridor to the ending location based on the third Bezier curve and not collide with a boundary of the second corridor:

3. the method of claim 1 or 2, further comprising: establishing a nonlinear programming model, taking the steering path of the vehicle in the step 4 as an initial solution, and obtaining a final steering path according to the nonlinear programming model, wherein the final steering path is obtained

The nonlinear programming model includes a second decision variable, which is the same as the decision variable in step 3, a second objective function, which is the same as the objective function in step 3, and second constraint conditions, which include the start position constraint and the end position constraint in step 3, the position continuity constraint at the curve junction, and the yaw angle continuity constraint, and further includes: the method comprises the following steps of starting and ending yaw angle constraints, curvature continuous constraints at the curve connection part, maximum curvature constraints and optimized obstacle avoidance constraints.

4. The method of claim 3, wherein the starting and ending yaw angle constraints are:

wherein, theta_iniIs the yaw angle, theta, of the vehicle at the starting position_terIs the yaw angle of the vehicle at the end position.

5. The method of claim 3, wherein the curvature continuity constraint at the curve junction is:

P_i，5-2P_i，4+P_i，3＝P_i+1，2-2P_i+1，1+P_i+1，0，i＝1，2

the maximum constraint on curvature is:

κ_i，j(s)≤κ_max，s∈(0，1)

wherein, κ_maxIs the maximum curvature value of the vehicle;

6. the method of claim 3, wherein optimizing obstacle avoidance constraints comprises:

first optimization obstacle avoidance constraint:

and second optimization obstacle avoidance constraint:

and (3) third optimization obstacle avoidance constraint:

wherein (x)_f，y_f) And (x)_r，y_r) The centers of the two enveloping circles are respectively obtained by calculating according to the following formula when the two enveloping circles cover the outline of the vehicle:

wherein L is_xIs the wheelbase of the vehicle, L_fIs the front overhang length of the vehicle, L_rIs the rear overhang length of the vehicle.

Technical Field

The invention relates to the technical field of intelligent networked automobiles, in particular to a method for planning a path of a steering road section based on a Bezier curve.

Background

With the rapid development of electronic technology and artificial intelligence algorithm, the technology accumulation in the field of automatic driving is more and more abundant. The application scene of automatic driving mainly comprises a structured road and a non-structured road, wherein common tasks of the structured road comprise lane changing driving and steering driving.

As a key technology in an automatic driving system, path planning determines a driving route of a vehicle, and has a high influence on safety and comfort of the vehicle. In the existing path planning technology, in order to satisfy curvature continuity constraint, a Bezier curve is often adopted to plan a path. However, most methods cannot guarantee the obstacle avoidance requirement of the Bezier curve, and certain potential safety hazards exist.

Disclosure of Invention

It is an object of the present invention to provide a method for path planning of a turn section based on bezier curves that overcomes or at least alleviates at least one of the above-mentioned drawbacks of the prior art.

In order to achieve the above object, the present invention provides a method for planning a path of a steered section based on bezier curves, the steered section including a first road before a vehicle is steered and a second road after the vehicle is steered, the steered path of the vehicle being planned with three bezier curves, a first bezier curve being located on the first road, a second bezier curve connected to the first bezier curve being located in an overlapping area of the first road and the second road, and a third bezier curve connected to the second bezier curve being located on the second road, the method comprising:

step 1, determining an initial state and a termination state of path planning; wherein the starting state comprises a longitudinal and lateral coordinate of a starting position of the vehicle and a yaw angle of the vehicle at the starting position, and the ending state comprises a longitudinal and lateral coordinate of an ending position of the vehicle and a yaw angle of the vehicle at the ending position;

step 2, establishing a first quadrilateral model based on the starting position and the ending position of the vehicle and the boundary of the first road and the second road, shrinking the edge of the first quadrilateral model, taking a shrunk second quadrilateral model as a corridor model, including a first corridor in the first road and a second corridor in the second road, wherein the first corridor and the second corridor are overlapped at a turning position, and the shrinking width is R_sWherein

Wherein L is_wIs the width of the vehicle, L_xIs the wheel base of the vehicle, L_RIs the rear overhang length of the vehicle;

the expressions of the four edges of the first corridor are as follows:

a₁₁x+b₁₁y+c₁₁＝0，a₁₂x+b₁₂y+c₁₂＝0，a₁₃x+b₁₃y+c₁₃＝0，a₁₄x+b₁₄y+c₁₄＝0；

the expressions of the four sides of the second corridor are as follows:

a₂₁x+b₂₁y+c₂₁＝0，a₂₂x+b₂₂y+c₂₂＝0，a₂₃x+b₂₃y+c₂₃＝0，a₂₄x+b₂₄y+c₂₄＝0；

based on the starting and ending positions of the vehicle, the boundaries of the first and second roads, and the contracted width R_sCan determine the coefficient a₁₁、b₁₁、c₁₁、a₁₂、b₁₂、c₁₂、a₁₃、b₁₃、c₁₃、a₁₄、b₁₄、c₁₄And coefficient a₂₁、b₂₁、c₂₁、a₂₂、b₂₂、c₂₂、a₂₃、b₂₃、c₂₃、a₂₄、b₂₄、c₂₄A value of (d);

the corridor model is marked as T₁And T₂：

Step 3, establishing an optimization model, wherein the optimization model comprises the following steps: decision variables, objective functions and constraint conditions; the decision variable is a control point coordinate of the Bezier curve, the Bezier curve is a quintic Bezier curve, and the control point coordinate comprises P_ij(x_i，j,y_i，j) I-1, 2,3, j-0, 1, … 5, wherein P is_ij(ii) the j +1 th control point representing the ith curve, (x)_i，j,y_i，j) Coordinates of the j +1 th control point representing the ith curve; the target function is the total length of the three Bezier curves; the constraint conditions comprise initial position constraint and final position constraint, position continuous constraint and yaw angle continuous constraint at the curve connection part, and obstacle avoidance constraint; calculating coordinate values of a control point when the total length of the three Bezier curves is shortest based on the decision variables, the objective function and the constraint conditions, and obtaining the three Bezier curves according to the relation between the coordinate of the control point and the Bezier curves, wherein the relation between the coordinate of the control point and the Bezier curves is as follows:

wherein s ∈ (0,1) is a continuous variable;

and 4, taking the three Bezier curve tracks obtained in the step 3 as the steering path of the vehicle.

Wherein the expression of the objective function is:

wherein the content of the first and second substances,

the matrix M satisfies:

the matrix Q satisfies:

the starting position constraint and the ending position constraint are as follows:

wherein x is_iniAnd y_iniRespectively the abscissa, x, of the starting position of the vehicle_terAnd y_terRespectively, the horizontal and vertical coordinates of the end position of the vehicle;

the position of the curve connection is continuously constrained as follows:

P_i，5＝Pi_+1,0, i＝1,2

the continuous constraint of the yaw angle at the curve junction is as follows:

P_i，5-P_i，4＝P_i+1，1-P_i+1，0, i＝1,2

the obstacle avoidance constraint includes:

a first obstacle avoidance constraint that limits the vehicle from traveling to an overlapping area of the first corridor and the second corridor based on the first Bezier curve and from colliding with a boundary of the first corridor:

a second obstacle avoidance constraint that limits the vehicle to pass through an overlapping region of the first corridor and the second corridor based on the second Bezier curve and not collide with a boundary of the two corridors:

a third obstacle avoidance constraint that limits the vehicle to traverse the second corridor to the ending location based on the third Bezier curve and not collide with a boundary of the second corridor:

optionally, the method may further include: establishing a nonlinear programming model, taking the steering path of the vehicle in the step 4 as an initial solution, and obtaining a final steering path according to the nonlinear programming model, wherein the final steering path is obtained

The nonlinear programming model includes a second decision variable, which is the same as the decision variable in step 3, a second objective function, which is the same as the objective function in step 3, and second constraint conditions, which include the start position constraint and the end position constraint in step 3, the position continuity constraint at the curve junction, and the yaw angle continuity constraint, and further includes: the method comprises the following steps of starting and ending yaw angle constraints, curvature continuous constraints at the curve connection part, maximum curvature constraints and optimized obstacle avoidance constraints.

Wherein the starting yaw angle constraint and the ending yaw angle constraint are:

wherein, theta_iniIs the yaw angle, theta, of the vehicle at the starting position_terIs the yaw angle of the vehicle at the end position.

Wherein the curvature continuity constraint at the curve junction is:

P_i，5-2P_i，4+P_i，3＝P_i+1，2-2P_i+1，1+P_i+1，0,i＝1,2

the maximum constraint on curvature is:

κ_i，j(s)≤κ_max,s∈(0,1)

wherein, κ_maxIn order to be a preset value, the device is provided with a power supply,

wherein the optimizing obstacle avoidance constraints comprises:

first optimization obstacle avoidance constraint:

and second optimization obstacle avoidance constraint:

and (3) third optimization obstacle avoidance constraint:

wherein (x)_f,y_f) And (x)_r,y_r) The centers of the two enveloping circles are respectively obtained by calculating according to the following formula when the two enveloping circles cover the outline of the vehicle:

by adopting the method provided by the embodiment of the invention, the initial state and the termination state of the path planning are set, and the model and the limiting conditions of the path planning are established, so that the obstacle avoidance requirement of the Bezier curve can be ensured, and the requirement of the path planning can be met.

Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by the practice of the invention.

Drawings

Fig. 1 is a schematic flow chart of a method for planning a route of a turning road section based on a bezier curve according to an embodiment of the present invention.

Fig. 2 is a road diagram of a turn section.

Fig. 3 and 4 are diagrams of corridor model generation.

Fig. 5 is a schematic diagram of a vehicle outline covered with two enveloping circles.

FIG. 6 is a schematic diagram of a path planned with three quintic Bezier curves in an embodiment of the present invention.

Detailed Description

The invention is described in detail below with reference to the drawings, which form a part hereof, and which are shown by way of illustration, embodiments of the invention. However, it should be understood by those skilled in the art that the following examples are not intended to limit the scope of the present invention, and any equivalent changes or modifications made within the spirit of the present invention should be considered as falling within the scope of the present invention.

The embodiment of the invention provides a method for planning a path of a turning road section based on a Bezier curve. The method takes the control point coordinates of the Bezier curve as decision variables, converts the path planning problem into an optimization solving problem through optimization modeling, and utilizes quadratic planning to solve an initial solution, thereby accelerating the solving efficiency and completing path planning.

Fig. 1 illustrates a method for planning a path of a turning road segment based on a bezier curve according to an embodiment of the present invention. The turning road section comprises a first road before the vehicle turns and a second road after the vehicle turns, three Bezier curves are used as turning paths of the vehicle, the first Bezier curve is positioned on the first road, the second Bezier curve connected with the first Bezier curve is positioned in an overlapping area of the first road and the second road, and the third Bezier curve connected with the second Bezier curve is positioned on the second road. The method comprises steps 100-500.

Step 100, determining the starting state and the ending state of the path planning. Wherein the starting state comprises a longitudinal and lateral coordinate of a starting position of the vehicle and a yaw angle of the vehicle at the starting position, and the ending state comprises a longitudinal and lateral coordinate of an ending position of the vehicle and a yaw angle of the vehicle at the ending position.

And 200, converting the road into a corridor model, and solving the corridor model.

Step 300, establishing an optimization model, and calculating to obtain the optimal Bezier curve control point coordinates so as to obtain three Bezier curves;

and step 400, taking the three Bezier curves obtained in the step 300 as the steering paths of the vehicle.

After step 400, may further include:

and 500, establishing a nonlinear programming model, taking the vehicle steering path in the step 400 as an initial solution, and solving to obtain a final steering path.

The following is detailed.

In step 100: the starting state and the ending state of the path plan are determined. The starting state comprises a horizontal ordinate of the starting position of the vehicle and a yaw angle of the vehicle at the starting position, and the ending state comprises a horizontal ordinate of the ending position of the vehicle and a yaw angle of the vehicle at the ending position.

Let the initial state of the vehicle be (x)_ini,y_ini,θ_ini) The termination state is (x)_ter,y_ter,θ_ter). Wherein x_ini,y_iniIs the abscissa, theta, of the starting position of the vehicle_iniIs the yaw angle at the starting position of the vehicle. x is the number of_ter,y_terIs the abscissa, theta, of the end position of the vehicle_terIs the yaw angle at the end position of the vehicle.

In step 200, the road is converted into a corridor model and the corridor model is solved.

Fig. 2 shows a road diagram of a turn section. And setting line segments AC and BD as two boundaries of a first road, wherein AC is a road inner side boundary and BD is a road outer side boundary. EC and FD are two boundaries of a second road, wherein EC is a road inside boundary, and FD is a road outside boundary. And C and D are intersections of the inner and outer boundaries of the two road boundaries, respectively.

Let D be the origin of coordinates, BD be the X-axis, extend AC until it intersects FD, let the intersection point be C₁. Extending EC until intersecting BD, making the intersection point C₂. To ensure a safe distance, let the quadrilateral ABDC₁And quadrilateral EFDC₂Shrinking with a shrinking width R_sThe contracted quadrangles are respectively A 'B' C₁'D' and E 'F' D 'C'₂As shown in fig. 3 and 4.

Determining R by_s: radius when two enveloping circles are used to cover the contour of the vehicle is R_s. Fig. 5 shows a schematic diagram of a vehicle contour covered with two enveloping circles. Wherein (x)_b,y_b) Is the center coordinate of the back enveloping circle, (x)_f,y_f) Is the center coordinate of the front enveloping circle.

Wherein L is_wIs the width of the vehicle, L_xIs the wheelbase of the vehicle, L_RThe rear overhang length of the vehicle.

In fig. 3 and 4, the coordinates of point a, point B, point C, point D, point E, and point F are all provided as known information by the positioning system and/or the map module in the autopilot system. Let the coordinate of point A be (x)_A，y_A) And the coordinate of the point B is (x)_B，y_B) And the coordinate of the point C is (x)_C，y_C) D point coordinate is (x)_D，y_D) The coordinate of point E is (x)_E，y_E) Coordinate of point F is (x)_F，y_F)。

Contracted quadrilateral A ' B ' C '₁D' the respective vertex coordinates can be expressed as: the coordinate of the point A 'is (x'_A，y'_A) And the coordinates of the point B 'are (x'_B,y'_B)，C'₁Point coordinates ofD 'point coordinate is (x'_D,y'_D). Quadrilateral E 'F' D 'C'₂The respective vertex coordinates may be expressed as: e 'point coordinate is (x'_E,y'_E) And the coordinate of the point F 'is (x'_F,y'_F)，C'₂Point coordinates ofThe coordinate calculation method is as follows:

x'_A＝x_A+R_s

y'_A＝y_A-R_s

x'_B＝x_B+R_s

y'_B＝y_B+R_s

x'_D＝x_D-R_s

y'_D＝y_D+R_s

x'_E＝x_E+R_s

y'_E＝y_E-R_s

x'_F＝x_F-R_s

y'_F＝y_F-R_s

note quadrilateral A ' B ' C '₁The expression of each side of D' is:

expression (c): a is₁₁x+b₁₁y+c₁₁＝0；

l_A'B'Expression (c): a is₁₂x+b₁₂y+c₁₂＝0；

l_B'D'Expression (c): a is₁₃x+b₁₃y+c₁₃＝0；

Expression (c): a is₁₄x+b₁₄y+c₁₄＝0

Four sides E ' F ' D ' C ' of the shrunk quadrilateral '₂Can be expressed as:

l_E′F′expression (c): a is₂₁x+b₂₁y+c₂₁＝0

Expression (c): a is₂₂x+b₂₂y+c₂₂＝0

Expression (c): a is₂₃x+b₂₃y+c₂₃＝0

l_D'F'Expression (c): a is₂₄x+b₂₄y+c₂₄＝0

Wherein, with a straight line l_A'C'For example, the coefficients a, b, and c of the linear model are calculated as follows:

and the two contracted quadrangles are the safe corridor model. Recording two safety corridors as T respectively₁And T₂：

In step 300, decision variables are determined, an objective function and corresponding constraints are constructed to obtain an optimization model, and a steering path is obtained by solving.

In the embodiment of the invention, the path is planned by three quintic Bezier curves.

FIG. 6 is a schematic diagram of a path planned with three quintic Bezier curves in an embodiment of the present invention. Wherein the first curve is U₁U₂Located within the quadrilateral A 'B' C 'D' and acting to guide the vehicle from a starting position U₁Proceed to the turn. Note that U₁The abscissa of (a) may also be the same as or different from the points a 'and B', in relation to the position on the vehicle selected for calculating the path, and is not limited herein. The second curve is U₂U₃Located in quadrangles A 'B' C 'D' and E 'F' D 'C'₂In the overlap region, the vehicle is guided to turn. The third curve is U₃U₄Located on quadrilateral E 'F' D 'C'₂Inside, guide the vehicle to the target point U₄I.e. the path termination location. The point U₁，U₂，U₃And U₄For illustrative purposes only, the specific coordinates thereof will be found in the optimization model described below.

The optimization model comprises the following steps: decision variables, objective functions and constraints. The decision variable is a variable to be optimized, in this embodiment, the decision variable is a control point coordinate of a bezier curve, the bezier curve is a quintic bezier curve, and the control point coordinate includes P_ij(x_i，j,y_i，jI-1, 2,3, j-0, 1, … 5, wherein P is_ij(ii) the j +1 th control point representing the ith curve, (x)_i，j,y_i，j) Coordinates of the j +1 th control point representing the ith curve; the relation between the control point coordinates and the Bezier curve is as follows:

wherein s ∈ (0,1) is a continuous variable.

The quintic bezier curve is a spline curve, which can be understood specifically as: after the positions of the six control points are determined, a curve generated by the six control points can be represented by a quintic polynomial. As can be seen from the above, for a bezier curve, the coordinates of the control points determine the shape of the curve. To obtain the optimal curve, the optimal control point coordinates are obtained.

In the embodiment of the invention, the control point coordinate x of the Bezier curve is used_ijAnd y_ijAnd as a decision variable, obtaining an optimal control point, and further obtaining an optimal Bezier curve as a vehicle steering path.

The objective function is a function of the decision variables, and when the objective function takes a minimum value, the decision variables are optimized. In the embodiment of the present invention, the objective function is the total length of the three bezier curves, the decision variable is optimal when the steering path is shortest, and the total length of the three bezier curves can be represented as:

wherein the matrix M satisfies:

the matrix Q satisfies:

the constraint conditions comprise a starting position constraint and an ending position constraint, a position continuous constraint and a yaw angle continuous constraint at a curve connection part, and an obstacle avoidance constraint.

To ensure that the starting position constraint and the ending position constraint are satisfied, a boundary state constraint is formed:

in order to ensure the position continuity of the curve connection part, position continuous constraint is formed:

P_i，5＝Pi_+1，0,i＝1,2

in order to ensure the continuity of the yaw angle at the curve joint, a yaw angle continuity constraint is formed:

P_i，5-P_i，4＝P_i+1，1-P_i+1，0,i＝1,2

in order to ensure that no collision occurs, obstacle avoidance constraints are respectively applied to control points of the three curves:

curve 1 should guide the vehicle to the overlapping area of the first corridor and the second corridor, and not collide with the boundary of the first corridor, so the obstacle avoidance constraint is:

curve 2 should guide the vehicle through the overlapping area of the first corridor and the second corridor and not collide with the boundary of the two corridors, so the obstacle avoidance constraint is:

curve 3 should guide the vehicle through the second corridor to the target point and not collide with the corridor boundary, so the obstacle avoidance constraint is:

based on the objective function, the decision variables and the constraint conditions, the optimization model can be converted into the following form:

and (3) solving the formulas (1) and (2) by adopting a Lagrange method to obtain the coordinates of the control points. And then calculating to obtain the Bezier curve according to the formula of the control point and the curve. As described above, in step 400, the calculated three bezier curves are used as the steering path of the vehicle.

In step 500, a nonlinear programming model is established, and the final steering path is obtained by solving the initial path in step 400 as an initial solution.

The nonlinear programming model includes a second decision variable, a second objective function, and a second constraint. The second decision variables are the same as the decision variables in step 300, the second objective function is the same as the objective function in step 300, the second constraint conditions include a start position constraint and an end position constraint in step 300, a position continuity constraint at a curve connection, and a yaw angle continuity constraint, and the method further includes: the method comprises the following steps of starting yaw angle constraint and ending yaw angle constraint, curvature continuous constraint at curve connection, curvature maximum constraint and optimization obstacle avoidance constraint.

Yaw angle theta at starting point of Bezier curve_i，iniYaw angle θ at termination point_i，terThe expression is as follows:

thus, the yaw angle constraint is:

and the continuity constraint aspect comprises a position continuity constraint, a yaw angle continuity constraint and a curvature continuity constraint. Wherein the position continuous constraint and the yaw angle continuous constraint are the same as in step 300, and the curvature continuous constraint is as follows:

P_i，5-2P_i，4+P_i，3＝P_i+1，2-2P_i+1，1+P_i+1，0,i＝1,2

the maximum constraint on curvature is:

κ_i，j(s)≤κ_max,s∈(0,1)

wherein, κ_maxThe preset value is the maximum curvature value of the vehicle.

The optimization obstacle avoidance constraint is to respectively constrain each curve:

for the first curve, the obstacle avoidance constraint is as follows:

for the second curve, the obstacle avoidance constraint is as follows:

for the third curve, the obstacle avoidance constraint is as follows:

wherein (x)_f,y_f) And (x)_r,y_r) The centers of the two enveloping circles in fig. 5 are respectively obtained by calculating decision variables:

wherein L is_xIs the wheelbase of the vehicle, L_fIs the front overhang length of the vehicle, L_rIs the rear overhang length of the vehicle.

q in one implementation, since s ∈ (0,1) is a continuous variable in the above optimization model, s is discretized, such thatAnd the constraint is introduced, so that a discrete constraint expression can be obtained as follows:

wherein:

C(x_i，j,y_i，j) In the general form of an inequality constraint, B (x)_i，j,y_i，j) In the general form of an equality constraint.

And (3) solving the formula (3) by adopting a sequential quadratic programming algorithm and taking the initial path obtained in the step 400 as an initial solution of the optimization model to obtain an optimal decision variable:i.e. the control point abscissa and ordinate of the bezier curve. And substituting the obtained control point coordinates into the following formula to obtain the final steering path coordinates:

by adopting the method provided by the embodiment of the invention, the initial state and the termination state of the path planning are set, and the model and the limiting conditions of the path planning are established, so that the obstacle avoidance requirement of the Bezier curve can be ensured, and the requirement of the path planning can be met.