A motion planning method for human-machine co-driving vehicles based on model predictive control

Abstract

The present invention relates to a motion planning method for a human-machine co-driving car based on model predictive control, which belongs to the technical field of motion planning for autonomous driving, and specifically to a motion planning method for a human-machine co-driving car based on model predictive control and suitable for multi-lane scenarios. When constructing a motion planning problem based on model predictive control in the method of the present invention, an optimization problem with constraints is constructed to calculate the trajectory within a set time period in the future. Based on the improved MPCC method, first, the cost function considers: the cost of tracking the recommended path, the comfort cost, the cost of the recommended speed, and the cost of human input; secondly, linear constraints, considering input constraints and vehicle state constraints; finally, nonlinear constraints, including: collision avoidance constraints with left and right boundaries, rollover constraints, and collision constraints with other vehicles.

Description

Technical Field

[0001] The present invention relates to a motion planning method for a human-machine co-driving car based on model predictive control, belonging to the technical field of motion planning for autonomous driving, and specifically to a motion planning method for a human-machine co-driving car based on model predictive control and applicable to multi-lane scenarios. Background Art

[0002] Autonomous driving is one of the most popular research directions in the field of artificial intelligence. Typical autonomous driving technology can be divided into the following levels: environmental perception and understanding, decision-making and planning, and bottom-level control. The decision-making and planning layer, as the "brain", plays a decisive role in the entire autonomous driving technology. The decision-making and planning part can be divided into behavioral decision-making and motion planning. Among them, motion planning is the most basic and important module, so it has become a research hotspot. Its main task is to consider the obstacles around the car, the physical constraints of the car and traffic rules, and plan a safe driving trajectory for the smart car under the guidance of the global navigation route, that is, a series of discrete waypoints with speed and direction information. As a new type of smart car, human-machine co-driving cars can give humans more driving rights than fully autonomous cars, integrate human decisions into the autonomous driving system, and ensure driving safety while improving the driver's driving experience.

[0003] Existing motion planning methods for autonomous vehicles can be divided into four categories: input space discretization method, random sampling planning method, end-to-end machine learning method, and optimization method. The present invention belongs to the optimization method, which overcomes the problem of insufficient security of the first type of method, overcomes the problem of poor trajectory continuity and difficulty in ensuring optimality of the second type of method, and solves the problem of insufficient interpretability of the fourth type of method. The present invention abstracts the trajectory planning problem of the car into an optimization problem with various constraints, can consider various real constraints and performance indicators, has high security, can be used for motion planning of human-machine co-driving cars, and can automatically select the best lane for driving. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide a motion planning method for a human-machine co-driving car based on model predictive control, which takes into account collision avoidance with other surrounding cars or road boundaries, performance limitations of the car itself, etc., to plan the best driving trajectory for the human-machine co-driving car. This method is suitable for multi-lane scenarios and takes some traffic rules into consideration.

[0005] The motion planning method of a human-machine co-driving car based on model predictive control of the present invention is mainly divided into six steps:

[0006] Step 1: Data reading;

[0007] Extract the center of mass of the human-machine co-driving car, extend the set value forward from the center of mass point along the road reference line, such as 100 meters, to obtain the left and right boundaries of the drivable area, the lane number, the number of lanes, the position of the center line of each lane, and also obtain the acceleration input by the human driver, the front wheel angle, and the position, angle, and speed information of other surrounding vehicles;

[0008] Step 2: Construction of the recommended path topology network (lattice graph);

[0009] Starting from the center of mass of the human-machine co-driving car, the center lines of each lane are sampled at set intervals and named as endpoints, thus forming many rows of endpoints;

[0010] The endpoints between the rows are regularly connected using a smooth cubic polynomial curve, the curve being a cubic polynomial of curvature with respect to the arc length, and the angle and curvature of the curve at the endpoint are consistent with the center line of the lane;

[0011] Thus, a recommended path topology network G consisting of multiple cubic polynomial curves and multiple rows of endpoints is obtained;

[0012] Step 3: Selection of recommended path;

[0013] First, evaluate each curve. Randomly select a cubic polynomial curve in the recommended path topology network G and perform uniform sampling evaluation on it. The sampling interval is 0.5 meters and named as evaluation points. The cost of the curve is considered: the cost of the curvature change rate, the curvature cost, the lane change cost, the number of evaluation points in the non-drivable area O, and the number of evaluation points that collide with other surrounding cars (around the center of mass of other cars). The number of evaluation points within the distance);

[0014] After obtaining the cost of each cubic polynomial curve, use A on the topological network G * The algorithm or Dijkstra algorithm calculates the shortest path from the car's centroid position to the last row of endpoints and uses it as the "recommended path";

[0015] Step 4: Select the recommended speed;

[0016] Taking into account the vehicle's current speed, lane speed limit, stop sign and other information, the recommended speed is calculated according to certain rules;

[0017] Step 5: Construct a motion planning problem based on model predictive control;

[0018] Construct a constrained optimization problem to calculate the trajectory in the future based on the improved MPCC (model predictive contouring control) method. First, the cost function considers: the cost of tracking the recommended path, the cost of comfort, the cost of recommended speed, and the cost of human input;

[0019] The second is linear constraints, which consider input constraints (including upper and lower limit constraints of acceleration, upper and lower limit constraints of front wheel angle change rate), vehicle state constraints (including upper and lower limit constraints of task area, upper and lower limit constraints of speed, upper and lower limit constraints of angle, upper and lower limit constraints of front wheel angle, etc.).

[0020] Finally, there are nonlinear constraints, including: collision avoidance constraints with the left and right boundaries, rollover constraints, and collision constraints with other vehicles;

[0021] Step 6: Problem solving;

[0022] The problem is solved using an existing nonlinear optimization solver. The initial solution is set to all 0s, and the planned trajectory can be solved. The trajectory contains position information, speed information, angle information, and front wheel angle information and acceleration information under the "bicycle model". After the car has traveled along the planned trajectory for a certain period of time (generally reaching the first planned trajectory waypoint), steps one to six are re-executed to continuously optimize the motion trajectory, thereby achieving autonomous driving.

[0023] Beneficial Effects

[0024] (1) The present invention can take into account various real constraints and performance indicators of the car, has high safety, can be used for motion planning of human-machine co-driving cars, and can automatically select the best lane for driving, and has high applicability in various driving scenarios.

[0025] (2) In the method of the present invention, the surrounding environment information includes the left and right boundaries of the drivable area, the lane number, the number of lanes, the position of the center line of each lane, and also includes the acceleration input by the human driver, the front wheel steering angle, and the position, angle, and speed information of surrounding vehicles.

[0026] (3) When the method of the present invention obtains information on the left and right boundaries of the drivable area, the lane number, the number of lanes, and the position of the center line of each lane, it extends 80-120 meters forward.

[0027] (4) In the method of the present invention, starting from the center of mass position of the human-machine co-driving car, the center line of each lane is sampled at a set distance and named as an endpoint, thereby forming a large number of rows of endpoints; a smooth cubic polynomial curve is used to regularly connect the endpoints between rows, and the curve is a cubic polynomial of curvature with respect to the arc length, and the angle and curvature of the curve at the endpoint are consistent with the center line of the lane; thereby, a recommended path topology network G composed of multiple cubic polynomial curves and multiple rows of endpoints is obtained.

[0028] (5) In the method of the present invention, when evaluating each curve in the recommended path topology network, any cubic polynomial curve is selected in the recommended path topology network G, and uniform sampling and evaluation is performed on it. The sampling intervals are named as evaluation points. The cost of each curve is considered: the cost of the curvature change rate, the curvature cost, the lane change cost, the number of evaluation points in the non-drivable area O, and the number of evaluation points that collide with other surrounding vehicles. After obtaining the cost of each cubic polynomial curve, the topology network G is evaluated using A. * The algorithm or Dijkstra algorithm calculates the shortest path from the car's centroid position to the last row of endpoints and uses it as the "recommended path".

[0029] (6) When constructing a motion planning problem based on model predictive control in the method of the present invention, an optimization problem with constraints is constructed to calculate the trajectory within a set time period in the future. Based on the improved MPCC method, first, the cost function considers: the cost of tracking the recommended path, the comfort cost, the cost of the recommended speed, and the cost of human input; secondly, linear constraints are considered, including input constraints and vehicle state constraints; finally, nonlinear constraints are considered, including: collision avoidance constraints with the left and right boundaries, rollover constraints, and collision constraints with other vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 A schematic diagram of a car model and a "car coordinate system" of a motion planning method of the present invention;

[0031] Figure 2 This is a schematic diagram of the data that needs to be collected for a motion planning method of the present invention. The light blue area is the drivable area Ω of the car, and Ω has two left and right boundaries. There is a road reference line between the two lanes in opposite directions, and the road reference line is the benchmark for describing the lane information;

[0032] Figure 3 A schematic diagram of sampling points and endpoints in a recommended path generation algorithm of a motion planning method of the present invention, wherein yellow points are sampling points located on a road reference line, and red points are endpoints located on a lane center line;

[0033] Figure 4 A schematic diagram of three basic connection modes in a recommended path generation algorithm of a motion planning method of the present invention;

[0034] Figure 5 Schematic diagram of other car elliptical packages in a recommended path generation algorithm of a motion planning method of the present invention, wherein is the speed of the i-th car around, a i is the semi-major axis of the ellipse, is the minimum length of the major semi-axle that encloses the vehicle;

[0035] Figure 6 The continuous rolling planning result of a motion planning method of the present invention on a U-shaped two-lane highway. DETAILED DESCRIPTION

[0036] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0037] Explanation of symbols

[0038] 1) If Figure 1 As shown, the human-machine co-driving car is in a two-dimensional plane A two-dimensional right-handed coordinate system is established with the direction pointed by the front of the vehicle as the x-axis and the center of mass of the vehicle as the origin, named the "own vehicle coordinate system". Motion planning is performed in the "own vehicle coordinate system", and the number of trajectory points / planning cycles is m. At the kth planning cycle / trajectory point (k = 0, 1, ..., m), the center of mass position coordinate of the human-machine co-driving vehicle is marked as (x k ,y k ), the longitudinal velocity along the front direction is v k , the angle between the front direction and the x-axis is φ k , the front wheel turning angle is δ k , the mileage of the car from time 0 to time k is s k . Use a 6-tuple to describe the state of the car at the kth planning cycle / trajectory point, denoted as z k ={x k ,y k , v k ,φ k , δ k ,s k If k = 0, the car is in the initial state, and according to the definition of the "car coordinate system", we know that x 0 =0,y 0 =0,φ 0 =0,s 0 = 0. The length from the center of the front axle to the center of mass of the vehicle is l f , the length from the center of the rear axle to the center of mass of the car is l r , the width of the car is W V , the length of the car is L V ;

[0039] The motion model of the car is the "kinematic bicycle model". where u k is the input (including acceleration and the angular velocity of the front wheel ), Δt k is the time interval of the kth planning period:

[0040]

[0041] Generally speaking, in order to ensure the accuracy and real-time performance of planning, Δt k The typical value is 0.05s to 0.2s, and the number of planning cycles / trajectory points m>20.

[0042] 2) Describe the surrounding roads according to the OPENDRIVE format, such as Figure 2 As shown in the figure, a road can be composed of multiple lanes, and there is a road reference line between two lanes in opposite directions. The road reference line has the number 0, and each lane has a corresponding number extending outward from the road reference line, which is 1, 2, 3, etc. The numbers of the lanes in the opposite direction increase with a negative sign. The drivable area Ω is defined as a combination of a series of lanes. The car can change lanes between the lanes within Ω. The boundaries of Ω are generally road reference lines, shoulders, etc. The non-drivable area is defined as The part that does not belong to Ω is denoted as O.

[0043] A motion planning method for a human-machine co-driving car based on model predictive control comprises the following steps:

[0044] Step 1: Data reading;

[0045] like Figure 3 As shown in the figure, the road information in OPENDRIVE format (including the left and right boundaries of the drivable area Ω, the lane numbers in Ω, the number of lanes, and the position of the center lines of each lane) is read and converted to the "own vehicle coordinate system" to find the point R on the road reference line that is closest to the center of mass of the human-machine co-driving car. 0 , starting from this point, extend 100 meters along the road reference line in the direction of the car's travel. Read the position of the surrounding cars in the "own vehicle coordinate system" angle speed length width The superscript i represents the i-th car. At this moment, the acceleration input by the human driver is The front wheel steering angle input by the human driver is

[0046] Step 2: Construction of the recommended path topology network (lattice graph);

[0047] like Figure 3 As shown, from R 0 Start along the road reference line in the direction of the car's travel, every Δs c The road reference line is sampled according to the speed v of the car. 0 Set the number of sampling points to Q (generally, the car speed v 0 The larger the Δs c The larger the value, the larger Q is, but too large a Q will slow down the execution speed of the algorithm). The sampling points are recorded as R 1 , R 2 , …, R Q , for each sampling point R i (i=1, 2, ..., Q) perform the following operations in sequence: Find the distance R between the center lines of each lane i The nearest point is recorded as the endpoint of the i-th row: (Assuming that at the sampling point R i There is ρ i In the "own vehicle coordinate system", read the angle, curvature, and lane number of each end point of the lane centerline in the i-th row, respectively:

[0048] The 0th row of sampling points is the center of mass of the car (i.e. the origin of the "car coordinate system"), and the curvature is Take the endpoint of the i-th row and two endpoints of the j-th row (i, j = 0, 1, 2, ..., Q, and i ≠ j) respectively and and They can be connected using a smooth cubic polynomial curve, which is a cubic polynomial of curvature with respect to arc length. The angle and curvature of the curve at the endpoints are: and and By constraining the angle and curvature at the endpoints, a unique curve can be obtained through the Newton iteration method. Curves using the Newton iteration method need to be error checked, and curves that trigger errors are not connected. The following three errors are defined:

[0049] Iteration number error: If the algorithm has more than 30 iterations, this error is triggered

[0050] Ring Formation Error: This error is triggered if the curve calculated by the algorithm forms a ring

[0051] Singular value error: This error is triggered if a singular value error occurs in the inversion operation of the matrix in the algorithm. Then the connection strategy is used. In order to connect the endpoints between the i-th row and the j-th row, three basic connection modes are first defined, such as Figure 4 As shown:

[0052] Direct connection mode: only connect two endpoints with the same lane number

[0053] Indirect connection mode: only connect two endpoints with different lane numbers, and the difference between the lane numbers is no more than 1;

[0054] Full connection mode: connect all possible endpoint combinations between the i-th row and the j-th row, and the difference in lane numbers is no more than 1;

[0055] Follow these steps to connect the endpoints between the endpoints of row i and row j:

[0056] Step 1: Set ρ 0 =1,ρ -1 =0,ρ -2 = 0. Then proceed to step 2.

[0057] Step 2: Set q = 1. If ρ q =ρ q-1 , then the endpoints in the qth row and the q-1th row are directly connected, otherwise the endpoints in the qth row and the q-1th row are fully connected. Then execute step 3.

[0058] Step 3: If ρ q =ρ q-2 , then the endpoints in the qth row and the q-2th row are in a non-direct connection mode, otherwise the endpoints in the qth row and the q-2th row are in a full connection mode. Then execute step 4.

[0059] Step 4: If ρ q =ρ q-3 , then the endpoints in row q and row q-3 are connected in a non-direct mode. Then go to step 5.

[0060] Step 5: If q<Q, then q=q+1 and execute step 2, otherwise end.

[0061] Thus, a recommended path topology network G consisting of multiple cubic polynomial curves and multiple rows of endpoints is obtained.

[0062] Step 3: Selection of recommended path;

[0063] First, evaluate each curve and select any cubic polynomial curve in G. The length of the cubic polynomial curve is denoted as l. len The lane numbers of the two endpoints are I 1 and I 2 . Perform uniform sampling evaluation on it, with a sampling interval of 0.5 meters. Assume that the curve has w evaluation points, and the curvature of the jth (j = 1, 2, ..., w) evaluation point is k j, the position in the "own vehicle coordinate system" is p j The position set of all evaluation points in the “vehicle coordinate system” is denoted as p 1：w ={p 1 , ..., p w}.

[0064] The cost of the curve is calculated as follows:

[0065] The cost of the rate of change of curvature:

[0066] Curvature cost:

[0067] Lane change cost: J lane =|I 1 -I 2 |

[0068] The number of evaluation points in the non-drivable area O: J bound =card(p 1：w ∩O), by calculating the distance between the evaluation point and the left and right boundaries, it can be determined whether the evaluation point is within the non-drivable area O.

[0069] The number of evaluation points for collision with other cars around (around the center of mass of other cars The number of evaluation points within the distance): in It is the position of the center of mass of the i-th surrounding car in the “own vehicle coordinate system”.

[0070] The cost of this curve is:

[0071] J=α 1 ·J dcurv +α 2 ·J curv +α 3 ·J lane +α 4 ·J bound +α 5 ·J dynamic

[0072] The parameter α 1 , ..., α 5 >0.

[0073] After obtaining the cost of each cubic polynomial curve, the A* algorithm or Dijkstra algorithm is used on the topological network G to calculate the shortest path from the 0th row to the Qth row endpoint (that is, the shortest path from the center of mass of the vehicle to the Qth row endpoint), and use it as the "recommended path". The "recommended path" can guide the car into an uncrowded lane and comply with traffic regulations.

[0074] The position (including horizontal and vertical coordinates), angle, and distance from the left and right boundaries of Ω of the "recommended path" in the "own vehicle coordinate system" can be calculated through discrete evaluation points. The position, angle, and distance from the left and right boundaries of Ω of the "recommended path" are obtained using a piecewise fitting method. The independent variable of the function is the arc length s of the "recommended path" starting from the position of the own vehicle. r , respectively: X(s r ), Y(s r ), Φ(s r ), B l (s r ), B r (s r )

[0075] Step 4: Select the recommended speed

[0076] The current speed of the car is v 0 The speed limit of each lane at the end of the "recommended road" is v limit , then the recommended speed v ref =min{v 0 +6, v limit}, if there is a stop sign ahead of the car, set the recommended speed to 0.

[0077] Step 5: Construct a motion planning problem based on model predictive control

[0078] This step requires constructing a constrained optimization problem to calculate the trajectory in the future based on the improved MPCC method. The first is the cost function. The cost function of the kth planning period is:

[0079] The cost of tracking the recommended path: J r (z k )=β 1 ·(e′ c ) 2 ,in:

[0080] e′ c =(x k -X(s k -e l (s k )))sinΦ(s k -e l (s k ))-(y k -Y(s k -e l (s k )))cosΦ(s k -e l (s k )),

[0081] e l (s k )=-(x k -X(s k ))cosΦ(s k )-(y k -Y(s k ))sinΦ(s k )

[0082] Comfort cost:

[0083] Recommended speed cost: J s (z k )=β 3 ·(v k -v ref ) 2

[0084] The cost of human input:

[0085] The total cost of the kth planning cycle is:

[0086] J k =(1-exp(-β 5 k))·(J r (z k )+J c (z k ,u k )+J s (z k ))+β 6 exp(-β 5 k)·J h (z k ) where β 1 , β 21 , β 22 , β 23 , β 3 , β 41 , β 42 , β 5 , β 6 >0.

[0087] The second is the linear constraint, for any k = 0, 1, ..., m, including: the input constraint u min ≤u k ≤u max (The upper and lower limits of acceleration Upper and lower limits of the front wheel steering angle change rate ), the constraint z of the car state min ≤z k ≤z max(The upper and lower limits of the task area x min ≤x k ≤x max and min ≤y k ≤y max , upper and lower speed constraints v min ≤v k ≤v max 、Angle upper and lower limit constraints -π≤φ k ≤π, upper and lower limits of the front wheel steering angle δ min ≤δ k ≤δ max , the lower limit constraint of mileage k ≥0).

[0088] Finally, there are nonlinear constraints, for any k = 0, 1, ..., m, including:

[0089] Collision avoidance constraints with the left and right boundaries:

[0090] e′ c -B r (s k -e l (s k ))-W≥0

[0091] -e′ c +B l (s k -e l (s k ))-W≥0

[0092] in

[0093]

[0094] Rollover restraint:

[0095] The description of the collision constraints with other cars is as follows:

[0096] like Figure 5 As shown, the outline of the i-th surrounding car is surrounded by an ellipse, the center of the ellipse is the center of mass of the i-th surrounding car, and the major semi-axis a of the ellipse is i The minor axis is b i , the long semi-axle keeps a certain length, and the length of the short semi-axle changes with the speed of other vehicles around, satisfying in is the minimum length of the major semi-axis surrounding the car. The area occupied by the ellipse is recorded as The car is surrounded by N circles, which are evenly distributed along the axis of the car, and the radius of the circle is R V , these circles are denoted as The constraints are all: By calculation Center of circle and O i The distance to the boundary can be determined With O i Is there any overlap?

[0097] Step 6: Problem Solving

[0098] If the total planning period is m, the number of other cars around is n, and the number of circles required to surround the car is N, the final optimization problem to be solved is:

[0099]

[0100] Stu min <u k <u max

[0101] z min <z k <z max

[0102] z k+1 =F(z K ,u K )

[0103]

[0104] B r (s k -e l (s k ))+W≤e′ c ≤B l (s k -e l (s k ))-W

[0105]

[0106] Use the existing nonlinear optimization solver to solve the problem, and set the initial solution to all 0s to solve the planned trajectory z 0 , z 1 , ..., z m The trajectory includes position information, speed information, angle information, and front wheel angle information and acceleration information under the "bicycle model". After the car has traveled along the planned trajectory for a certain period of time (generally reaching the first planned trajectory waypoint), steps 1 to 6 are re-executed to continuously optimize the motion trajectory, thereby achieving autonomous driving. Figure 6 A schematic diagram of continuous motion planning of this method on a U-shaped two-lane highway is shown.

Claims

1. A motion planning method for a human-machine co-driving car based on model predictive control, characterized in that the steps include: Step 1: Starting from the center of mass of the human-machine co-driving car, extend forward along the road reference line to obtain surrounding environment information; Step 2: Construct the recommended path topology network; Step 3: Evaluate each curve in the recommended path topology network constructed in step 2 to obtain the cost of each curve, and select the recommended path according to the cost of each curve; Step 4: Calculate the recommended speed according to the known rules based on the vehicle's current speed, lane speed limit, and stop sign information; Step 5: Construct a motion planning problem based on model predictive control; Step 6: Solve the motion planning problem constructed in step 5 to obtain the planned trajectory; In the step 1, the surrounding environment information includes the left and right boundaries of the drivable area, the lane number, the number of lanes, the position of the center line of each lane, and also includes the acceleration input by the human driver, the front wheel angle, and the position, angle, and speed information of surrounding vehicles; In the step 2, the method for constructing the recommended path topology network is: Starting from the center of mass of the human-machine co-driving car, the center lines of each lane are sampled at set intervals and named as endpoints, thus forming many rows of endpoints; The endpoints between the rows are regularly connected using a smooth cubic polynomial curve, the curve being a cubic polynomial of curvature with respect to the arc length, and the angle and curvature of the curve at the endpoint are consistent with the center line of the lane; Thus, a recommended path topology network G consisting of multiple cubic polynomial curves and multiple rows of endpoints is obtained; In the step 3, when evaluating each curve in the recommended path topology network, any cubic polynomial curve is randomly selected in the recommended path topology network G, and uniform sampling evaluation is performed on it. The sampling intervals are named as evaluation points. The cost of each curve is considered: the cost of the curvature change rate, the curvature cost, the lane change cost, the number of evaluation points in the non-drivable area O, and the number of evaluation points that collide with other surrounding cars. After obtaining the cost of each cubic polynomial curve, the topology network G is evaluated using A * The algorithm or Dijkstra algorithm calculates the shortest path from the car's centroid position to the last row of endpoints and uses it as the "recommended path"; The cost of the curve is calculated as follows: The cost of the rate of change of curvature: Curvature cost: Lane change cost: J lane =|I1-I2| Among them, l len is the length of the curve, w is the number of evaluation points of the curve, k k is the curvature of the kth evaluation point, k k-1 is the curvature of the k-1th evaluation point, I1 and I2 are the lane numbers of the two end points of the curve; The number of evaluation points in the non-drivable area O: J bound =card(p 1：w ∩O), by calculating the distance between the evaluation point and the left and right boundaries, determine whether the evaluation point is within the non-drivable area O; Among them, p 1：w is the position set of all evaluation points in the "own vehicle coordinate system"; Number of evaluation points for collisions with other surrounding cars: in is the position of the center of mass of the i-th surrounding car in the "own car coordinate system"; p j is the position of the jth evaluation point in the "own vehicle coordinate system", j = 1, 2, 3, ... w; The cost of this curve is: J=α1·J dcurv +α2·J curv +α3·J lane +α4·J bound +α5·J dynamic Wherein parameters α1, ..., α5> 0; In step 4, the recommended speed v ref =min{v0+6,v limit }, the current speed of the car is v0, and the speed limit of the lane at each endpoint of the "recommended road" is v limit , then if there is a stop sign in front of the car, the recommended speed is set to 0; In the step 5, when constructing the motion planning problem based on model predictive control, a constrained optimization problem is constructed to calculate the trajectory within a future set time period. Based on the improved MPCC method, first, the cost function considers: the cost of tracking the recommended path, the cost of comfort, the cost of recommended speed, and the cost of human input; The second is linear constraints, which consider the constraints of input and the state of the car; Finally, there are nonlinear constraints, including: collision avoidance constraints with the left and right boundaries, rollover constraints, and collision constraints with other vehicles; The input constraints include upper and lower limits of acceleration and upper and lower limits of the rate of change of the front wheel steering angle. The vehicle state constraints include upper and lower limits of the task area, upper and lower limits of the speed, upper and lower limits of the angle, and upper and lower limits of the front wheel steering angle. The cost of tracking the recommended path: Jr(z k )=β1·(e′c) 2 ,in: e′ c =(x k -X(s k -e l (s k )))sinΦ(s k -e l (s k ))-(y k -Y(s k -e l (s k )))cosΦ(s k -e l (s k )), e l (s k )=-(x k -X(s k ))cosΦ(s k )-(y k -Y(s k ))sinΦ(s k ) Comfort cost: Recommended speed cost: J s (z k )=β3·(v k -v ref ) 2 The cost of human input: The total cost of the kth planning cycle is: J k =(1-exp(-β5k))·(J r (z k )+J c (z k ,u k )+J s (z k ))+β6exp(-β5k)·J h (z k ) Where β1, β 21 ,b 22 ,b 23 ,β3,β 41 ,b 42 , β5, β6>0; The second is the linear constraint, for any k = 0, 1, ..., m, including: the input constraint u min ≤u k ≤u max , upper and lower limits of acceleration Upper and lower limits of the front wheel steering angle change rate Constraints on car states z min ≤z k ≤z max , upper and lower limit constraints x of the task area min ≤x k ≤x max and min ≤y k ≤y max , upper and lower speed constraints v min ≤v k ≤v max 、Angle upper and lower limit constraints -π≤φ k ≤π, upper and lower limits of the front wheel steering angle δ min ≤δ k ≤δ max , the lower limit constraint of mileage k ≥0; Finally, there are nonlinear constraints, including collision avoidance constraints with the left and right boundaries: e′ c -B r (s k -e l (s k ))-W≥0 -e′ c +B l (s k -e l (s k ))-W≥0 in Rollover restraint: The center of mass of the human-machine co-driving car is marked as (x k ,y k ), the longitudinal velocity along the front direction is v k , the angle between the front direction and the x-axis is φ k , the front wheel turning angle is δ k , the mileage of the car from time 0 to time k is s k , z k ={x k ,y k , v k ,φ k , δ k ,s k ], the length from the center of the front axle to the center of mass of the vehicle is l f , the length from the center of the rear axle to the center of mass of the car is l r , the width of the car is W V ; X( ), Y( ), Φ( ), B l ( ),B r ( ) is the function of the position, angle, and distance from the left and right boundaries of Ω of the recommended path obtained by fitting; is the acceleration, is the angular velocity of the front wheel, acceleration input for the human driver; Front wheel steering angle for human driver input; In the step six, the problem is solved using an existing nonlinear optimization solver, the initial solution is set to all 0, and the planned trajectory is solved. The trajectory includes position information, speed information, angle information, and front wheel angle information and acceleration information under the "bicycle model". When the car drives along the planned trajectory and reaches the first planned trajectory waypoint, steps one to six are re-executed to continuously optimize the motion trajectory, thereby achieving autonomous driving.

2. The motion planning method for a human-machine co-driving car based on model predictive control according to claim 1, characterized in that: When obtaining information on the left and right boundaries of the drivable area, the lane number, the number of lanes, and the position of the center line of each lane, extend 80-120 meters forward.

Images