Intelligent vehicle transverse and longitudinal control method based on kinematics recursion in lane changing scene

Abstract

The invention belongs to the technical field of automatic driving testing, and particularly relates to an intelligent vehicle transverse and longitudinal control method based on kinematics recursion in a lane changing scene. Comprising the following steps: step 1, constructing a Frenet coordinate system; step 2, calculating state quantities of parameters S and L for describing vehicle pose information in the Frenet coordinate system; 3, performing transverse trajectory planning on the intelligent vehicle based on the two-degree-of-freedom kinematics model; 4, longitudinal trajectory planning is carried out; 5, establishing a two-degree-of-freedom dynamic model; and step 6, designing an MPC controller. According to the invention, the real-time operation of the vehicle in the ring and in the real vehicle can be ensured, and the method has important significance for the transverse and longitudinal high-precision and high-efficiency real-time application of the intelligent vehicle.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving testing technology, specifically a method for lateral and longitudinal control of intelligent vehicles based on kinematic recursion in lane changing scenarios. Background Technology

[0002] In recent years, autonomous driving technology has received widespread attention, with lane changing in complex scenarios being a hot research topic. However, achieving safe and comfortable autonomous lane changing in complex traffic environments still faces many challenges.

[0003] In traditional manual driving mode, drivers face high operational risks during lane changes, and reasonable lane change planning and control technology is crucial to improving driving safety.

[0004] In recent years, autonomous driving technology has gradually transitioned from laboratory research to practical road applications.

[0005] In autonomous driving systems, lane-changing safety relies on reasonable trajectory planning and precise control strategies. Good trajectory planning not only needs to meet the constraints of the starting and ending points (such as speed and acceleration), but also requires reasonable constraints on the intermediate processes of the trajectory to ensure that it conforms to the vehicle's kinematic characteristics, avoiding planning failures or collision risks caused by unreasonable trajectories. Simultaneously, the controllability of the trajectory directly affects the vehicle's execution performance, thus determining the overall safety and smoothness of lane changes. Therefore, adopting trajectory planning methods that better conform to vehicle dynamics constraints can improve the stability and safety of lane changes and reduce the risk of loss of control. This is precisely one of the core challenges in autonomous driving lane-changing planning research. Summary of the Invention

[0006] To address the aforementioned issues, this invention provides a kinematic recursive-based intelligent vehicle lateral and longitudinal control method for lane-changing scenarios, ensuring that lateral errors can be effectively controlled to an acceptable range within speeds up to 50 km / h, while also providing good comfort.

[0007] The technical solution of this invention is described below in conjunction with the accompanying drawings:

[0008] This invention provides a method for lateral and longitudinal control of intelligent vehicles based on kinematic recursion in lane change scenarios, characterized by the following steps:

[0009] Step 1: Construct the Frenet coordinate system;

[0010] Step 2: Calculate the horizontal and vertical coordinate state variables describing the vehicle's pose information in the Frenet coordinate system;

[0011] Step 3: Perform lateral trajectory planning for the intelligent vehicle based on a two-degree-of-freedom kinematic model;

[0012] Step 4: Perform longitudinal trajectory planning through cruise sampling, following sampling, and overtaking sampling;

[0013] Step 5: Establish a two-degree-of-freedom dynamic model;

[0014] Step 6: Design the MPC controller.

[0015] Furthermore, the specific method for step one is as follows:

[0016] 11) Set the origin of the coordinate system to 0, the S direction to the vertical axis of the coordinate system, and the L direction to the horizontal axis; the given coordinate (S,L) represents the distance S of the vehicle's projection on the reference line from the origin, and the perpendicular distance L between the vehicle and the projection point.

[0017] 12) Establish a Cartesian coordinate system, setting T as the vehicle trajectory and R as the reference line, as shown below:

[0018]

[0019] In the formula, and Let θ be the unit orthogonal vector of point P; t Let P be the heading angle; Let x be the coordinates of point P; r Let y be the x-coordinate of point P; r Let P be the ordinate of point P; and Let Q be the unit orthogonal vector of point Q;

[0020] 13) Assume AB is the distance between two points on the reference line, and the arc length between AB is ds. Then we have:

[0021]

[0022] 14) and The triangle formed by the vectors has the following condition when ds→→0:

[0023]

[0024] Export at this point:

[0025]

[0026] Therefore, the basic formula for transforming from the Cartesian coordinate system to the Frenet coordinate system is derived as follows:

[0027]

[0028] Furthermore, the specific method for step two is as follows:

[0029] 21) From s = s r Export:

[0030] In the formula, l is the horizontal coordinate; s is the vertical coordinate;

[0031] 22) Differentiation yields:

[0032]

[0033] Therefore, we can conclude that:

[0034]

[0035] Multiply both sides of the equation by their dot product get:

[0036]

[0037] Multiply both sides of the equation by their dot product get:

[0038]

[0039] Therefore, we can conclude that:

[0040]

[0041] In the formula, for The derivative;

[0042] Finally, the following was released:

[0043]

[0044] In the formula, Let be the second derivative of s;

[0045] twenty three) Neglecting this, we conclude:

[0046]

[0047] Furthermore, the specific method for step three is as follows:

[0048] 31) Establish a two-degree-of-freedom kinematic model so that a trajectory can be generated given the initial state and control parameters;

[0049] Based on geometric and velocity relationships, we can deduce:

[0050]

[0051] In the formula, x, y, and θ are the horizontal and vertical coordinates and the heading angle, respectively; v is the longitudinal speed.

[0052] The theorem leads to:

[0053]

[0054] In the formula, δ f β is the front wheel steering angle; β is the center of gravity deviation angle; R is the vehicle turning radius; δ r Let be the front wheel steering angle; a and b are the distances from the center of gravity to the front and rear axles, respectively. Since the vehicle's weight affects the position of the center of gravity, the lengths of a and b change. Introducing the wheelbase L, we get:

[0055]

[0056] At low speeds, the car will not slide laterally, due to v y ≈0, therefore Rear-wheel steering is not studied, therefore δ r =0, then:

[0057]

[0058] The state-space equations are as follows:

[0059]

[0060] in, D = 0

[0061] x = [x s y s θ s ],u=δ

[0062] The state-space equations are discretized using the following formula:

[0063]

[0064] Discretize matrices A and B:

[0065]

[0066] In the formula, dt is the discrete time interval; A d A is the discretized matrix; B d The matrix is ​​the discretized form of B;

[0067] 32) The control parameters are obtained through Newton's iteration to ensure that the trajectory endpoint accurately reaches the given endpoint state, as follows:

[0068] 321) Initialize the optimization variables: θ = θ0;

[0069] 322) Loop until the termination condition is met:

[0070] 3221) Calculate the gradient of the objective function:

[0071] 3222) Update and optimize variables:

[0072] 3223) Check termination conditions:

[0073] If the conditions are met;

[0074] 323) Return the final solution θ*;

[0075] 33) Design a state sampling strategy to ensure that the Newton-Raphson iteration method does not result in no solution or even a trajectory that does not conform to the driving intention, and generate a more continuous trajectory cluster for subsequent filtering, as follows:

[0076] In the vehicle coordinate system, a set of sampling points is generated as the endpoint state set of the trajectory; each sampling point contains state variables x, y, and θ, i.e., position and heading angle, thus forming the trajectory endpoint state set state{x, y, θ}; the starting state is set to {0, 0, 0}; however, to ensure the rationality of trajectory generation, the endpoint states need to be filtered and optimized, mainly in two stages:

[0077] The first stage is the screening stage, which eliminates states that do not meet the vehicle kinematic constraints.

[0078] The second stage is the optimization stage, which limits the range of change in trajectory curvature;

[0079] 34) Trajectory optimization based on Lagrange interpolation;

[0080] 341) For a trajectory, set the initial state of the vehicle to state{x} i y i θ i The final state is state{x}. target y target θ target}, {x i y i θ i}∈states, where states is the set of endpoint states generated by the sampling points; x i The x-coordinate of the initial state; y i θ represents the initial ordinate of the coordinate system; i The initial direction angle; x target The x-coordinate of the final state; y target θ represents the ordinate of the final state. target The direction angle of the final state;

[0081] 342) Assume that the steering wheel control sequence generated from the starting point to the ending point is k(t), and k0 is the initial value of the sequence k(t). m Let k(t) be the median of the sequence k(t). f Let p be the final value of the k(t) sequence. When the reference vehicle speed is v, the time length of the steering wheel angle sequence is time = s / v, where s is the arc length. At this time, let p = {k0, k...} m k f , s} are the key parameters; considering the smoothness of the steering wheel angle, Lagrange interpolation is used to calculate k0, k m k f The median value;

[0082] Let the interpolation function be δ=f(t), and the interpolation coordinates be (0,0) and (time / 2,k). m (time,k) f ), where, according to the Lagrange interpolation formula:

[0083]

[0084] In the formula, i represents the interpolation order, and we take i = 2; L i (t) is the interpolation basis function: y i is the ordinate of the i-th interpolation; n is the total number of interpolations;

[0085]

[0086] In the formula, t represents time; j For the interpolation of node j; t i For the interpolation of node i;

[0087] Substituting the data, we get:

[0088]

[0089] Substituting this into the quadratic Lagrange interpolation function, we have:

[0090]

[0091] Discretize the time, and obtain

[0092]

[0093] Among them, t k ∈[0,time]; Dividing the interval n into equal parts to obtain k, and using n=s / ds, we obtain the front wheel steering angle δ at each time point under the initial control parameters. k Then, based on the two-degree-of-freedom equations of the car, the state variables at each time step are derived:

[0094] The state-space equations are:

[0095]

[0096] In the formula, x c y is the x-axis at time t = time; c θ represents the ordinate at time t = time; c The direction angle at time t = time;

[0097] The state variable at time t = time is [x] c ,y c ,θ c The error between this point and the expected endpoint is:

[0098] E = [target] x -x c target y -y c target θ -θ c ]′

[0099] In the formula, target x The ideal x-coordinate; target y For target x The ideal ordinate; target θ For the ideal direction angle;

[0100] Therefore, we obtain the control parameters s and k from the given control parameters s and k. m k f The mapping to the state variable error E at time t = time is denoted as:

[0101] E=J(s,k m ,k f )

[0102] When the initial condition s = s i km = km i kf = kf i The time error is:

[0103] E c =J(s,k m ,k f )

[0104] The gradient of the mapping function with respect to the independent variable is:

[0105]

[0106] In the formula, h is a constant;

[0107] Let the Jacobian matrix be:

[0108]

[0109] According to the update formula of Newton's method, the gradient update direction is:

[0110] dp = -J -1 ·E c

[0111] Let the learning rate be α, then according to the update rule, update the control parameters {s km kf}:

[0112] p = p + α·dp

[0113] In the formula, p is the gradient;

[0114] If the error E c If the value is less than the threshold, the update stops; the final control parameter p = {k0, k...} m k f Substituting 's' into the discrete two-degree-of-freedom state-space equations of the car yields the trajectory connecting the starting point to the specified endpoint.

[0115] Furthermore, the specific method for step four is as follows:

[0116] 41) Conduct cruise sampling;

[0117] In the cruise scenario, since there are no obstacles on the path, the vehicle plans its longitudinal trajectory according to the target cruise speed. In this case, it is not necessary to strictly specify the target position s1; it is only necessary to ensure that a reasonable trajectory is generated given the endpoint speed v1 and acceleration a1. The longitudinal motion trajectory of the vehicle is constructed using a quartic polynomial:

[0118] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4

[0119] In the formula, s(t) is the trajectory; a0, a1, a2, a3 and a4 are constants;

[0120] Assume the current state of the vehicle is s = s0. t is the sampling trajectory time, v i Let be the final velocity of the trajectory, and the boundary conditions are as follows:

[0121]

[0122] In the formula, s0 is the current position; v0 is the current velocity.

[0123] Then the polynomial coefficients are solved, and the ST plot curve is determined.

[0124] 42) Conduct on-vehicle sampling;

[0125] At each sampling time point of the ST diagram, the end-point state is sampled, and the S value S is obtained at the rear foot of the obstacle. obs_rear_point A certain safety distance S is reserved below. safe_dis Then every S downwards sample_dis A point is sampled at a distance; since six boundary conditions need to be determined for the vehicle's initial state {s0, v0, a0} and terminal state {s1, v1, a1}, a fifth-degree polynomial is needed to construct the ST curve. The ST curve equation is as follows:

[0126] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5

[0127] In the formula, a0, a1, a2, a3, a4, and a5 are constants;

[0128] Assuming the vehicle in front has a speed of 25 km / h, and the vehicle's initial speed is 30 km / h with an initial acceleration of 0, the boundary conditions with a safe distance of 5m are set as follows:

[0129]

[0130] Substitute the boundary conditions into the ST curve equation to solve for the polynomial coefficients;

[0131] 43) Conduct sampling while overtaking;

[0132] The boundary conditions for trajectory planning are set as follows:

[0133]

[0134] Among them, S obs_front_point =26 is the position of the front heel of the vehicle in front, S overtake_front_dis =5 represents the safe overtaking distance; the overtaking sampling case has 6 boundary conditions, therefore a fifth-degree polynomial is constructed as follows:

[0135] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5

[0136] Substituting the boundary conditions into the fifth-degree polynomial allows us to solve for the polynomial coefficients.

[0137] Furthermore, the specific method for step five is as follows:

[0138] 51) By establishing the resultant force equilibrium equation of the vehicle in the y-axis direction and the moment equilibrium equation about the center of mass, a two-degree-of-freedom dynamic model is established, as shown in the following formula:

[0139]

[0140] In the formula, k1 is the front wheel lateral stiffness; k2 is the rear wheel lateral stiffness; β is ××; u is the longitudinal velocity; a is the distance from the front axle to the center of mass; b is the distance from the rear axle to the center of mass; δ is the steering angle; m is the mass; ω is the lateral acceleration; r For yaw rate gain; I z Let Z be the moment of inertia of the vehicle about the Z-axis;

[0141] By combining the two equations and eliminating v, the steady-state yaw rate gain can be obtained:

[0142]

[0143] Among them, w r yaw rate; L is the vehicle wheelbase; K is the understeer angle; u is the longitudinal speed; δ is the front wheel steering angle.

[0144] Furthermore, the specific method for step six is ​​as follows:

[0145] 61) In the vehicle coordinate system, the current pose of the vehicle is x=0, y=0, θ=0; In model predictive control, assuming an initial steering wheel angle is given, the vehicle will move to another position at the next moment. The pose at this time is in the vehicle coordinate system of the previous moment. The offset and heading angle at this time are calculated by the vehicle two-degree-of-freedom model.

[0146] Assuming the vehicle speed u remains constant, integrating both sides of the steady-state yaw rate gain gives:

[0147]

[0148] In the formula, w r θ0 is the yaw rate gain; L is the wheelbase; K is the stability factor; u is the longitudinal velocity; θ0 is the initial heading angle.

[0149] At this point, we obtain the state variable: heading angle θ; based on the small angle assumption, we have:

[0150]

[0151] If both sides travel at the same speed u, then:

[0152]

[0153] Integrating both sides, we get:

[0154]

[0155] At this point, the discrete state-space equation of the state variable x(k) = [θd]′ is obtained as follows:

[0156] x(k+1)=Ax(k)+Bu(k)+C

[0157] Where A = [1 1],

[0158] 62) Predict state variables;

[0159] The state variables from time k to k+Np are recursively calculated using the discretized state-space equations. The recursive process is as follows:

[0160]

[0161] In the formula, Np represents the prediction time domain;

[0162] 63) Establishing a secondary planning problem;

[0163] Based on the error between the predicted state variables and the planned trajectory, a quadratic programming problem is established to ultimately optimize the control input, enabling the vehicle to follow the planned trajectory. The formulas for calculating the heading angle error and lateral error are as follows:

[0164]

[0165] In the formula, k = 1, 2, ..., Np;

[0166] Construct an objective function that includes lateral error and heading angle error; the objective function is in the form of a weighted sum of squared errors, aiming to minimize the error at all times in the prediction time domain; assume that the weighting coefficients of the errors are Q and R, where Q is the weight of the lateral error θ. err (k+i) are weighted, and R is applied to the heading angle error d. err Weigh (k+i) and let x(k) = [θ err (k),d err [k], the objective function J is expressed as:

[0167]

[0168] In the formula, x(k) is the state variable; u(k) is the control variable;

[0169] The standard quadratic programming forms are as follows:

[0170]

[0171] In the formula, x is the optimization variable, i.e., the front wheel steering angle; H is the Hessian matrix; and g is a linear vector.

[0172] Consider the vehicle's control input constraints; these constraints mainly include the steering wheel angle (i.e., the front wheel angle) and the rate of change of the steering wheel angle; assume the upper and lower limits of the steering wheel angle are δ. min and δ max The control input constraints are expressed as:

[0173]

[0174] The beneficial effects of this invention are as follows:

[0175] 1) This invention constructs the Frenet coordinate system and proposes a planning framework that decouples the horizontal and vertical axes, which simplifies the modeling and solution complexity of the mapping algorithm. In terms of horizontal planning, it innovatively introduces a horizontal planning method based on kinematic model recursion, which improves the maximum curvature of the trajectory and the lateral comfort.

[0176] 2) This invention addresses the key issue of synergistic optimization of accuracy and comfort in trajectory tracking control of autonomous vehicles. It proposes a model predictive control method based on lateral dynamics modeling, which ensures that the control algorithm can effectively control the lateral error to an acceptable range within 50km / h and provides good comfort.

[0177] 3) This invention has high computational efficiency and can guarantee real-time operation in both the vehicle loop and the actual vehicle, which is of great significance for the real-time application of intelligent vehicles with high precision and efficiency in both the lateral and longitudinal directions. Attached Figure Description

[0178] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0179] Figure 1 This is a schematic diagram of the Frenet coordinate system;

[0180] Figure 2 This is a schematic diagram of the Cartesian-Frenet coordinate transformation.

[0181] Figure 3a Diagram of differential amplification Figure 1 ;

[0182] Figure 3b Diagram of differential amplification Figure 2 ;

[0183] Figure 4 This is a schematic diagram of the trajectory optimization algorithm process;

[0184] Figure 5 This is a schematic diagram of the differential magnification of a two-degree-of-freedom vehicle model.

[0185] Figure 6 This is a schematic diagram of the original sampling points;

[0186] Figure 7 Pruning at sampling points Figure 1 ;

[0187] Figure 8 Pruning at sampling points Figure 2 ;

[0188] Figure 9 This is a schematic diagram of the kinematic recursive trajectory;

[0189] Figure 10 This is a schematic diagram of cruise sampling ST.

[0190] Figure 11 This is a schematic diagram of ST sampling for vehicle following;

[0191] Figure 12 Schematic diagram of ST sampling for overtaking;

[0192] Figure 13 This is a schematic diagram of the vehicle's coordinate system;

[0193] Figure 14 This is a schematic diagram of the process of the present invention;

[0194] Figure 15a A comparison chart of the maximum curvature of the trajectories;

[0195] Figure 15b A comparison chart showing the trade-offs between lateral comfort and cost;

[0196] Figure 16 This is a first-level architecture diagram of a Simulink model;

[0197] Figure 17a This is a scene diagram of two moving lines;

[0198] Figure 17b This is a coordinate graph with two moving lines.

[0199] Figure 18a This is a schematic diagram of the front wheel steering angle curve;

[0200] Figure 18b This is a schematic diagram of the lateral acceleration curve;

[0201] Figure 18c The motion trajectory is plotted in XY mode;

[0202] Figure 18d This is a graph showing the yaw rate curve.

[0203] Figure 19a This is a diagram of the front wheel steering angle.

[0204] Figure 19b This is a lateral acceleration curve.

[0205] Figure 19c The motion trajectory is plotted in XY mode;

[0206] Figure 19d This is a graph showing the yaw rate curve.

[0207] Figure 20a This is a diagram of the front wheel steering angle.

[0208] Figure 20b This is a lateral acceleration curve.

[0209] Figure 20c The motion trajectory is plotted in XY mode;

[0210] Figure 20d This is a graph showing the yaw rate. Detailed Implementation

[0211] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0212] Example 1

[0213] See Figure 14 This embodiment provides a kinematic recursive-based intelligent vehicle lateral and longitudinal control method for lane changing scenarios, characterized by the following steps:

[0214] Step 1: Construct the Frenet coordinate system, as follows:

[0215] See Figure 1 Let be the Frenet coordinate system established based on the reference line. The origin of the coordinate system is O, the S direction is the vertical axis of the coordinate system, and the L direction is the horizontal axis. Therefore, the given coordinates (S, L) represent that the distance S from the origin of the vehicle's projection on the reference line, and the perpendicular distance L between the vehicle and the projection point.

[0216] 12) Establish a Cartesian coordinate system, setting T as the vehicle trajectory and R as the reference line, as shown below:

[0217]

[0218] In the formula, and Let θ be the unit orthogonal vector of point P; t Let P be the heading angle; Let x be the coordinates of point P;r Let y be the x-coordinate of point P; r Let P be the ordinate of point P; and Let Q be the unit orthogonal vector of point Q;

[0219] 13) See Figure 3a Assume A and B are two points close to each other on the reference line, and the arc length between A and B is ds. Then we have:

[0220]

[0221] 14) and The triangle formed by vectors is as follows Figure 3b As shown, when ds approaches 0, we have:

[0222]

[0223] Export at this point:

[0224]

[0225] Therefore, the basic formula for transforming from the Cartesian coordinate system to the Frenet coordinate system is derived as follows:

[0226]

[0227] Step 2: Calculate the horizontal and vertical coordinate state variables describing the vehicle pose information in the Frenet coordinate system, as follows:

[0228] 21) By Figure 2 We can conclude that: s = s r (7)

[0229] Thus, the following can be derived:

[0230] In the formula, l is the horizontal coordinate; s is the vertical coordinate;

[0231] 22) Differentiation yields:

[0232]

[0233] Substituting equations (7) and (8) into equation (9), we obtain:

[0234]

[0235] Multiply both sides of equation (10) by their respective dot products get:

[0236]

[0237] At this point, both sides of the equation are multiplied by the dot product. get:

[0238]

[0239] From equations (10), (11), and (12), we can deduce that:

[0240]

[0241] In the formula, for The derivative;

[0242] According to equations (12) and (13), we can obtain:

[0243]

[0244] From equations (12), (13), and (14), we can deduce that:

[0245]

[0246] In the formula, Let be the second derivative of s;

[0247] twenty three) The smaller value is negligible. Based on the above formula, we can deduce that:

[0248]

[0249] Thus, we obtained all the state expression for the Frenet coordinate system.

[0250] See Figure 4 Step 3: Perform lateral trajectory planning for the intelligent vehicle based on a two-degree-of-freedom kinematic model, as follows:

[0251] 31) Establish a two-degree-of-freedom kinematic model, such as 5, so that a trajectory can be generated given the starting state and control parameters;

[0252] Based on geometric and velocity relationships, we can deduce:

[0253]

[0254] In the formula, x, y, and θ are the horizontal and vertical coordinates and the heading angle, respectively; v is the velocity of the center of mass.

[0255] The theorem leads to:

[0256]

[0257] In the formula, δ f β is the front wheel steering angle; β is the center of gravity deviation angle; R is the vehicle turning radius; δr Let be the front wheel steering angle; a and b are the distances from the center of gravity to the front and rear axles, respectively. Since the vehicle's weight affects the position of the center of gravity, the lengths of a and b change. Introducing the wheelbase L, we get:

[0258]

[0259] At low speeds, it is assumed that the vehicle will not slide laterally, due to v y ≈0, therefore Furthermore, this application does not study rear-wheel steering, therefore there is δ r =0, then:

[0260]

[0261] The state-space equations are as follows:

[0262]

[0263] in,

[0264] x = [x s y s θ s ],u=δ (23)

[0265] The state-space equations are discretized using the following formula:

[0266]

[0267] Discretize matrices A and B:

[0268]

[0269] In the formula, dt is the discrete time interval; A d A is the discretized matrix; B d The matrix is ​​the discretized form of B;

[0270] 32) The control parameters are obtained through Newton's iteration to ensure that the trajectory endpoint reaches the given endpoint state precisely, as shown in Table 1:

[0271] Table 1. Pseudocode for Newton's Iteration Method

[0272]

[0273] 33) Design a state sampling strategy;

[0274] For any trajectory, the first step is to determine the states of the starting and ending points. The two-degree-of-freedom kinematic model introduced earlier allows for the generation of a trajectory given the starting state and control parameters. Newton's iteration method can then be used to determine these control parameters, ensuring the trajectory's endpoint precisely reaches the given ending state. Therefore, the purpose of this section is to establish a sampling point generation strategy to ensure that Newton's iteration method does not produce unsolvable trajectories or trajectories that severely contradict driving intentions, and to generate a more continuous cluster of trajectories for subsequent selection.

[0275] In the vehicle coordinate system, a set of sampling points is generated as the endpoint state set of the trajectory; each sampling point contains state variables x, y, and θ, i.e., position and heading angle, thus forming the trajectory endpoint state set state{x, y, θ}; the starting state is set to {0, 0, 0}; however, to ensure the rationality of trajectory generation, the endpoint states need to be filtered and optimized, mainly in two stages:

[0276] The first stage is the screening stage, which aims to eliminate states that do not meet the vehicle's kinematic constraints. These unreasonable states may cause trajectories that do not conform to vehicle kinematics or steering constraints, thus affecting the feasibility of the actual vehicle. The core of kinematic constraints is to ensure that the state variables conform to the vehicle's motion equations, such as maximum curvature or minimum turning radius, etc. Figure 6 As shown;

[0277] The second stage is the optimization stage, which further improves the quality of the state set. Specifically, to ensure the smoothness and safety of the trajectory, i.e., to avoid sharp turns, it is necessary to limit the range of curvature changes in the trajectory to prevent excessive fluctuations. This not only reduces the difficulty of vehicle control but also improves driving comfort and safety. Figure 7 As shown in the diagram above, remove coordinates such as (6, 5, 30°) and (6, -5, -30°). The black arrows represent the retained endpoint states. Then, add more states within a reasonable range to make the state set more continuous, as illustrated in the diagram below. Figure 8 As shown.

[0278] 34) Trajectory optimization based on Lagrange interpolation;

[0279] 341) For a trajectory, set the initial state of the vehicle to state{x} i y i θ i The final state is state{x}. target y target θ target}, {x i y i θ i}∈states, where states is the set of endpoint states generated by the sampling points; x i The x-coordinate of the initial state; y i θ represents the initial ordinate of the coordinate system; i The initial direction angle; x target The x-coordinate of the final state; y target θ represents the ordinate of the final state. target The direction angle of the final state;

[0280] 342) Assume that the steering wheel control sequence generated from the starting point to the ending point is k(t), and k0 is the initial value of the sequence k(t). m Let k(t) be the median of the sequence k(t). f Let p be the final value of the k(t) sequence. When the reference vehicle speed is v, the time length of the steering wheel angle sequence is time = s / v, where s is the path length. At this point, let p = {k0, k...} m k f , s} are the key parameters; considering the smoothness of the steering wheel angle, Lagrange interpolation is used to calculate k0, k m k f The median value;

[0281] Let the interpolation function be δ=f(t), and the interpolation coordinates be (0,0) and (time / 2,k). m (time,k) f ), where, according to the Lagrange interpolation formula:

[0282]

[0283] In the formula, i represents the interpolation order, and we take i = 2; L i (t) represents the interpolation basis function: the ordinate of the i-th interpolation; n is the total number of interpolations;

[0284]

[0285] In the formula, t represents time; j For the interpolation of node j; t i For the interpolation of node i;

[0286] Substituting the data, we get:

[0287]

[0288] Substituting this into the quadratic Lagrange interpolation function, we have:

[0289]

[0290] Discretizing the time interval, we get:

[0291]

[0292] Among them, t k ∈[0,time]; Dividing the interval n into equal parts to obtain k, and using n=s / ds, we obtain the front wheel steering angle δ at each time point under the initial control parameters. k Then, based on the two-degree-of-freedom equations of the car, the state variables at each time step are derived:

[0293] The established state-space equations are:

[0294]

[0295] The state variable at time t = time is [x] c ,y c ,θ c The error between this point and the expected endpoint is:

[0296] E = [target] x -x c targt y -y c target θ -θ c ]′

[0297] Therefore, we obtain the control parameters s and k from the given control parameters s and k. m k f The mapping to the state variable error E at time t = time is denoted as:

[0298] E=J(s,k m ,k f (34)

[0299] When the initial condition s = s i km = km i kf = kf i The time error is:

[0300] E c =J(s,k m ,k f (35)

[0301] The gradient of the mapping function with respect to the independent variable is:

[0302]

[0303] In the formula, h is a constant;

[0304] Let the Jacobian matrix be:

[0305]

[0306] According to the update formula of Newton's method, the gradient update direction is:

[0307] dp = -J -1 ·E c (38)

[0308] Let the learning rate be α, then update the control parameter {skmkf} according to the update rule:

[0309] p = p + α·dp (39)

[0310] In the formula, p is the gradient;

[0311] If the error E c If the value is less than the threshold, the update stops; the final control parameter p = {k0, k...} m k f Substituting and into the discrete two-degree-of-freedom state-space equations of the car yields the trajectory connecting the starting point to the specified endpoint. The resulting trajectory family is as follows: Figure 9 As shown (due to the large number of trajectories obtained through the sampling strategy, only a portion of the trajectories are displayed).

[0312] Step 4: Perform longitudinal trajectory planning through cruise sampling, following sampling, and overtaking sampling, as detailed below:

[0313] Longitudinal planning essentially involves planning the ST curve in the Frenet coordinate system. Based on the vehicle's current state (longitudinal position s0, longitudinal velocity v0, longitudinal acceleration a0) and the target state (target point's longitudinal position s1, longitudinal velocity v1, longitudinal acceleration a1), a polynomial curve is constructed on the ST graph to generate the planned trajectory. This trajectory construction uses the initial and final states as boundary conditions. For different driving scenarios, such as cruising, following, and overtaking, the longitudinal sampling strategy differs. The following sections will delve into and discuss these three scenarios separately.

[0314] 41) Conduct cruise sampling;

[0315] In the cruise scenario, since there are no obstacles on the path, the vehicle plans its longitudinal trajectory according to the target cruise speed. In this case, it is not necessary to strictly specify the target position s1; it is only necessary to ensure that a reasonable trajectory is generated given the endpoint speed v1 and acceleration a1. The longitudinal motion trajectory of the vehicle is constructed using a quartic polynomial:

[0316] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 (40)

[0317] In the formula, s(t) is the trajectory; a0, a1, a2, a3, and a4 are constants;

[0318] Assume the current state of the vehicle is s = s0. t is the sampling trajectory time, v i Let be the final velocity of the trajectory, and the boundary conditions are as follows:

[0319]

[0320] In the formula, s0 is the current position; v0 is the current velocity.

[0321] Substituting equation (41) into equation (40) yields the polynomial coefficients, thus determining the ST curve. For the research scenario of low-to-medium speed lane changes, to better reflect actual vehicle operating characteristics, this paper sets the trajectory final velocity sampling range to {10, 20, 30, 40, 50}, in m / s. The sampling trajectory time is set to {2, 4, 6, 8}. The polynomial coefficients are obtained by solving the equation using the boundary conditions of the initial and final states, and the ST curve is plotted in MATLAB software. Figure 10 As shown;

[0322] 42) Conduct on-vehicle sampling;

[0323] During the following vehicle sampling process, it is required that the S-value of the vehicle maintains a certain safe distance from the S-value of the vehicle in front, and that the vehicle speed at the end of the trajectory be consistent with the speed of the vehicle in front. This avoids the potential risk of collision with the vehicle in front. The specific sampling method involves sampling the end-point state at each sampling time point on the ST map, and recording the S-value at the rear foot of the obstacle. obs_rear_point A certain safety distance S is reserved below. safe_dis Then every S downwards sample_dis A point is sampled at a distance. Since six boundary conditions need to be determined—the initial state {s0, v0, a0} and the final state {s1, v1, a1}—a fifth-degree polynomial is required to construct the ST curve. The ST curve equation is as follows:

[0324] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (42)

[0325] In the formula, a0, a1, a2, a3, a4, and a5 are constants;

[0326] In this application, dynamic constraints are considered for the vehicle and the vehicle in front during lane changing. Assuming the vehicle in front has a speed of 25 km / h, the initial speed of the vehicle is 30 km / h, and the initial acceleration is 0, the boundary conditions with a safe distance of 5m are set as follows:

[0327]

[0328] Substituting the boundary conditions of equation (43) into equation (42) allows us to solve for the polynomial coefficients. The resulting ST curve is shown below. Figure 11 As shown.

[0329] In the figure, t k This indicates a time point, with values ​​in the range {1, 2, 3, 4, 5, 6, 7, 8} seconds. `follow_safe_dis` sets the safe distance to 5 meters. `obs_rear_point` indicates the longitudinal position of the rear foot of the preceding vehicle. k The sampling value is set to 5, meaning the trajectory endpoint is 5 meters further increased from the safe distance. By setting these boundary conditions, it is possible to effectively ensure that the vehicle maintains a safe distance from the vehicle in front during lane changes, while achieving a smooth transition. This setting provides the necessary boundary conditions for trajectory planning in a dynamic environment, generating a reasonable trajectory that satisfies the constraints using a polynomial construction method.

[0330] 43) Conduct sampling while overtaking;

[0331] In an overtaking scenario, assuming the vehicle in front is traveling at a significantly lower speed than the vehicle ahead, the vehicle needs to overtake by using a detour. To ensure a sufficient safe distance after overtaking and a smooth speed transition, the boundary conditions of the longitudinal trajectory planning need to be constrained. Specifically, the longitudinal position S at the trajectory endpoint must be greater than the longitudinal position of the vehicle ahead. Simultaneously, to prevent the safe distance from shortening after overtaking, the vehicle's speed at the trajectory endpoint must be the same as the vehicle ahead. Based on these requirements, the boundary conditions for trajectory planning are set as follows:

[0332]

[0333] Among them, S obs_front_point =26 is the position of the front heel of the vehicle in front, S overtake_front_dis =5 represents the safe overtaking distance. It can be seen that the overtaking sampling situation has 6 boundary conditions, so a fifth-degree polynomial can be constructed as follows:

[0334] s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (45)

[0335] Substituting the boundary conditions of equation (44) into equation (45) allows us to solve for the polynomial coefficients. The resulting ST curve is shown below. Figure 12 As shown.

[0336] As shown in the figure, the curves represent trajectories generated under different sampling endpoint conditions. At T=2s, the preceding vehicle begins to appear, and the vehicle's longitudinal trajectory is affected by the position of the preceding vehicle. The trajectory endpoint design takes into account safety distance requirements, meaning that the endpoint is increased by a safety distance based on the preceding vehicle's end position to ensure that the vehicle has sufficient space to avoid collision during lane changes. A series of possible trajectories can be generated by using different sampling endpoints.

[0337] Based on the reference path planning trajectory given above, the focus next is on tracking the reference trajectory. For longitudinal control, the planned vehicle speed is achieved through PID control. For lateral control, through research on lateral model predictive control based on a two-degree-of-freedom vehicle model, both control accuracy and comfort are considered, as detailed below:

[0338] Step 5: Establish a two-degree-of-freedom dynamic model, as follows:

[0339] 51) By establishing the resultant force equilibrium equation of the vehicle in the y-axis direction and the moment equilibrium equation about the center of mass, a two-degree-of-freedom dynamic model is established, as shown in the following formula:

[0340]

[0341] In the formula, k1 is the front wheel lateral stiffness; k2 is the rear wheel lateral stiffness; β is ××; u is the longitudinal velocity; a is the distance from the front axle to the center of mass; b is the distance from the rear axle to the center of mass; δ is the steering angle; m is the mass; ω is the lateral acceleration; r For yaw rate gain; I z Let Z be the moment of inertia of the vehicle about the Z-axis;

[0342] By combining the two equations and eliminating v, the steady-state yaw rate gain can be obtained:

[0343]

[0344] In the formula, ω r yaw rate, L is the vehicle wheelbase, K is the understeer, u is the longitudinal speed, and δ is the front wheel steering angle.

[0345] Step 6: Design the MPC controller, as detailed below:

[0346] 61) For example Figure 13 As shown, in the vehicle's coordinate system, the current vehicle pose is x = 0, y = 0, θ = 0. In model predictive control, assuming an initial steering wheel angle is given, the vehicle will move to... Figure 13 At this position, the pose is in the vehicle coordinate system of the previous moment. The offset and heading angle at this moment are calculated using the vehicle model.

[0347] Assuming the vehicle speed u remains constant, integrating both sides of equation (47) simultaneously yields:

[0348]

[0349] At this point, we obtain the state variable: heading angle θ. Based on the small angle assumption, we have:

[0350]

[0351] If both sides travel at the same speed u, then:

[0352]

[0353] Integrating both sides of equation (50), we get:

[0354]

[0355] At this point, the discrete state-space equation of the state variable x(k) = [θd]′ is obtained as follows:

[0356] x(k+1)=Ax(k)+Bu(k)+C (52)

[0357] Where A = [1 1],

[0358] 62) Perform state variable prediction;

[0359] The main idea behind state variable prediction is to derive the state variables within the prediction time domain (e.g., prediction range of time Np) using a recursive method based on the previously established discrete state-space equations. This process is based on the core idea of ​​MPC control, which is to calculate the optimal control input by predicting the state variables over a future period to ensure the accuracy of trajectory tracking and the stability of the system. Through the discretized state-space equations, the state variables from time k to k+Np in the future can be recursively calculated (where Np is the duration of the prediction time domain). The specific recursive process is as follows:

[0360]

[0361] 63) Establishing a secondary planning problem;

[0362] The state variables predicted from the current time k to the future time k+Np are obtained recursively through the discretized state-space equations. These predicted state variables, including the lateral position d(k+i) and heading angle θ(k+i), provide the basis for subsequent error calculation. Based on the errors between these predicted state variables and the planned trajectory, a quadratic programming problem is established to optimize the control input, enabling the vehicle to follow the planned trajectory as accurately as possible. The formulas for calculating the heading angle error and lateral error are as follows:

[0363]

[0364] Where k = 1, 2, ..., Np, to minimize the vehicle's deviation throughout the entire prediction time domain, we need to construct an objective function that includes lateral error and heading angle error. This objective function is typically a weighted sum of squared errors, aiming to minimize the error at all times within the prediction time domain. Assume the error weighting coefficients are Q and R, where Q corresponds to the lateral error θ. err (k+i) are weighted, and R is applied to the heading angle error d. err Weigh (k+i) and let x(k) = [θ err (k),d err [k], the objective function J can be expressed as:

[0365]

[0366] The standard quadratic programming forms are as follows:

[0367]

[0368] In the formula, x is the optimization variable, i.e., the front wheel steering angle, H is the Hessian matrix, and g is a linear vector.

[0369] In addition to the objective function, the quadratic programming problem also needs to consider the vehicle's control input constraints. In this invention, the constraints mainly include the steering wheel angle (i.e., the front wheel angle) constraint and the rate of change constraint of the steering wheel angle. Assume the upper and lower limits of the steering wheel angle are δ. min and δ max Furthermore, the rate of change of the steering wheel angle is constrained. These constraints ensure that the control input varies within a reasonable range, thereby avoiding unrealistic control behaviors and ensuring the vehicle's control stability and comfort. The control input constraints can be expressed as:

[0370]

[0371] In summary, this invention can ensure real-time operation in both the vehicle-in-the-loop and real-vehicle environments, which is of great significance for the real-time application of intelligent vehicles with high precision and efficiency in both lateral and longitudinal directions.

[0372] Example 2

[0373] This embodiment uses fifth-order polynomial and quadratic optimization as comparative planning algorithms and compares the performance of the three planning algorithms in open-loop simulation. The comparison metrics include maximum trajectory curvature and lateral comfort cost. Maximum curvature is the maximum curvature on the trajectory within a frame. This metric reflects the smoothness of the trajectory, especially in lane-changing scenarios. If this value is too large, it indicates that the trajectory is more difficult for the control execution level, and the potential control error will be larger. If this value is too small, it indicates a greater risk of collision. Therefore, a value within a reasonable range is considered effective. The second metric is the lateral comfort cost introduced in Section 2.6.1. This metric quantifies comfort through the lateral velocity and acceleration of the trajectory. A smaller value is better, assuming successful lane changing. Conversely, a larger value may lead to worse comfort and even larger control errors, resulting in lane-changing failure.

[0374] Experimental results are as follows Figure 15a and Figure 15b As shown in Table 2, at t = 1.8s, the three planners started planning simultaneously. The quadratic optimization method planned the trajectory that collided with the obstacle, but it remained ineffective after multiple adjustments to the weight parameters and solution parameters. The fifth-order polynomial method had the largest trajectory curvature at the initial moment, which decreased as lane changes occurred. At t = 4s, the results were almost consistent with those of the kinematic recursive method. The average maximum trajectory curvature of the kinematic recursive method throughout the process was 0.42, which was less than the maximum curvature of the fifth-order polynomial, and the maximum was 2.8, which was also less than the maximum value of the fifth-order polynomial. The two methods remained almost consistent after t = 4s.

[0375] Table 2 Comparison of Three Planning Algorithms

[0376]

[0377] like Figure 15a and Figure 15b The figure shows a comparison curve of the lateral trajectory cost for the three methods. The quadratic method, also due to parameter limitations, is weaker than the other two in this metric. The lateral comfort cost of the fifth-order polynomial method is still higher than that of the kinematic recursion method. As the lane change progresses, the lateral comfort costs of the two methods are almost identical at t=4s. The fundamental reason is that the fifth-order polynomial only constrains the initial and final states, without considering the changes in the intermediate trajectory states. However, parameters such as the curvature of the intermediate trajectory are determined by the polynomial coefficients, not the vehicle's kinematic parameters, thus leading to a larger curvature and lateral comfort cost. In contrast, each step of the kinematic recursion-based planning method is based on vehicle kinematic formulas, thus allowing for constraints on parameters such as curvature throughout the entire process. From the above analysis, we can conclude that:

[0378] (1) Lateral planning based on quadratic optimization method has a serious dependence on weight parameters and solution parameters, making it difficult to apply to complex and ever-changing scenarios.

[0379] (2) In the early stages of lane changes, the kinematic recursive method is superior to the quintic polynomial programming method in terms of trajectory curvature and lateral comfort. In the late stages of lane changes, the performance of the two methods is almost identical.

[0380] Example 3

[0381] To verify the effectiveness and good control accuracy of the Model Predictive Control (MPC) algorithm based on a two-degree-of-freedom vehicle kinematic model, this embodiment performs simulation verification of the algorithm. Key indicators such as lateral offset error and steering wheel angle curve are analyzed to evaluate the algorithm's performance. To ensure that the MPC algorithm based on the two-degree-of-freedom vehicle kinematic model can effectively track errors and provide good comfort, this section sets up an experimental environment. The experimental environment uses a co-simulation platform of Simulink and CarSim, where Simulink is used for algorithm implementation and simulation control, and CarSim is used to simulate real vehicle dynamics and road environments. This approach is used to evaluate the control effect of the MPC algorithm. The model is as follows: Figure 16 As shown:

[0382] A Model Predictive Control (MPC) algorithm was implemented in Simulink. Simulink obtains real-time vehicle state information from CarSim, such as x, y, θ, and v, for calculating the input information of the MPC algorithm, thereby outputting a steering wheel angle control sequence. CarSim then updates the vehicle state information based on the steering wheel angle output by Simulink, forming a closed-loop control. To test the algorithm's performance under high dynamic conditions, this experiment used a dual lane change scenario with vehicle speeds of 10 km / h, 30 km / h, and 50 km / h, simulating the vehicle's operation during lane changes. The road conditions were as follows: Figure 17a and Figure 17b As shown, the details are as follows:

[0383] a. Double lane change test at a speed of 10km / h;

[0384] like Figure 18a , Figure 18b , Figure 18c and Figure 18dAs shown, at a speed of 10 km / h, the vehicle exhibited a certain lateral deviation at the initial stage of lane changing, with a maximum lateral deviation of 10 cm at the start of the lane change. During lane change maintenance, the maximum lateral deviation error increased to 11 cm, while during lane change return, the maximum lateral deviation error was 10.5 cm. The front wheel steering angle curve showed a relatively stable trend throughout the simulation, with only slight vibrations at certain specific moments. Overall, the curve change was smooth. The front wheel steering angle curve began to change at 32 seconds, indicating that the vehicle began lane changing. Around 50 seconds, the front wheel steering angle reached its maximum value of 42 degrees, successfully entering the adjacent lane. Between 60 and 70 seconds, the front wheel steering angle gradually decreased, and the vehicle completed the lane change and returned to the original lane. Therefore, we can conclude that:

[0385] (1) The MPC algorithm based on the two-degree-of-freedom kinematic model of the vehicle has high accuracy in the lateral tracking process of double lane change at 10km / h, and can effectively control the trajectory error of the vehicle and ensure that the vehicle's trajectory is close to the target path.

[0386] b. Double lane change test at a speed of 30km / h;

[0387] like Figure 19a , Figure 19b , Figure 19c and Figure 19d As shown. At a vehicle speed of 30 km / h, the steering wheel angle output by the MPC is initially reversed at t = 9 s. While this initially produces significant heading and lateral displacement errors in the short prediction time domain, it reduces these errors over a longer prediction time domain. The peak steering wheel angle during the left lane change is 44.5 degrees, generating a lateral acceleration of approximately 0.14 m / s², indicating that this method effectively ensures both comfort and trajectory tracking accuracy. The maximum steering wheel angle generated during the lane change return process is approximately 42 degrees, with a similar lateral acceleration of approximately 0.14 m / s². Regarding tracking errors, the lateral error reaches its maximum at x = 100 m at the start of the lane change, with an error of 23 cm. The maximum error at the successful lane change is 20 cm, and the maximum lateral error at the lane change return and successful return is only 22 cm. Therefore, we can conclude that:

[0388] (1) The model predictive control algorithm based on the two-degree-of-freedom model of the vehicle can keep the tracking error within a small range while ensuring a certain level of comfort in the double lane change condition at 30km / h.

[0389] c. Double lane change test at a speed of 50km / h;

[0390] like Figure 20a , Figure 20b , Figure 20c and Figure 20d As shown, at a vehicle speed of 50 km / h, during a lane change, the maximum steering wheel angle is 96 degrees, resulting in a lateral acceleration of approximately 0.35 m / s². 2 The ride comfort is good. During lane change and return, the steering wheel angle is approximately 60 degrees, generating a maximum lateral acceleration of approximately 0.3 m / s². 2 During the entire lane change process, the maximum lateral offset error was approximately 0.3m. Therefore, the following conclusions can be drawn:

[0391] The model predictive control algorithm based on the two-degree-of-freedom model of the vehicle can keep the lateral error within an acceptable range in the double lane change condition at 50km / h, and can also ensure a certain level of comfort.

[0392] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for lateral and longitudinal control of an intelligent vehicle based on kinematic recursion in lane-changing scenarios, characterized in that, Includes the following steps: Step 1: Construct the Frenet coordinate system; Step 2: Calculate the horizontal and vertical coordinate state variables describing the vehicle's pose information in the Frenet coordinate system; Step 3: Perform lateral trajectory planning for the intelligent vehicle based on a two-degree-of-freedom kinematic model; Step 4: Perform longitudinal trajectory planning through cruise sampling, following sampling, and overtaking sampling; Step 5: Establish a two-degree-of-freedom dynamic model; Step 6: Design a model predictive controller based on the two-degree-of-freedom dynamics model of a car.

2. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane changing scenarios according to claim 1, characterized in that, The specific method for step one is as follows: 11) Set the origin of the coordinate system to 0, the S direction to the vertical axis of the coordinate system, and the L direction to the horizontal axis; the given coordinate (S,L) represents the distance S of the vehicle's projection on the reference line from the origin, and the perpendicular distance L between the vehicle and the projection point.

12. Establish a Cartesian coordinate system, setting T as the vehicle trajectory and R as the reference line, as shown below: In the formula, and Let be the unit orthogonal vector of point P; θ t Let P be the heading angle; Let x be the coordinates of point P; r Let y be the x-coordinate of point P; r Let P be the ordinate of point P; and Let Q be the unit orthogonal vector of point Q; 13) Assume that AB are two points on the reference line, and the arc length between AB is ds. Then we have: 14) and The triangle formed by the vectors, when ds→0, has: Export at this point: Therefore, the basic formula for transforming from the Cartesian coordinate system to the Frenet coordinate system is derived as follows:

3. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane changing scenarios according to claim 1, characterized in that, The specific method for step two is as follows: 21) From s = s r Export: In the formula, l is the horizontal coordinate; s is the vertical coordinate; 22) Differentiation yields: Therefore, we can conclude that: Multiplying both sides of the equation by their dot product get: Multiplying both sides of the equation by their dot product get: Therefore, we can conclude that: In the formula, for The derivative; Finally, the following was released: In the formula, Let be the second derivative of s; twenty three) Neglecting this, we conclude:

4. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane changing scenarios according to claim 1, characterized in that, The specific method for step three is as follows: 31) Establish a two-degree-of-freedom kinematic model so that a trajectory can be generated given the initial state and control parameters; Based on geometric and velocity relationships, we can deduce: In the formula, x, y, and θ are the horizontal and vertical coordinates and the heading angle, respectively; v is the longitudinal speed. The theorem leads to: In the formula, δ f β is the front wheel steering angle; β is the center of gravity deviation angle; R is the vehicle turning radius; δ r Let be the front wheel steering angle; a and b are the distances from the center of gravity to the front and rear axles, respectively. Since the vehicle's weight affects the position of the center of gravity, the lengths of a and b change. Introducing the wheelbase L, we get: At low speeds, the car will not slide laterally, due to v y ≈0, therefore Rear-wheel steering is not studied, therefore δ r =0, then: The state-space equations are as follows: in, D = 0 x=[x s y s i s ],u=δ The state-space equations are discretized using the following formula: Discretize matrices A and B: In the formula, dt is the discrete time interval; A d A is the discretized matrix; B d The matrix is ​​the discretized form of B; 32) The control parameters are obtained through Newton's iteration to ensure that the trajectory endpoint accurately reaches the given endpoint state, as follows: 321) Initialize the optimization variables: θ = θ0; 322) Loop until the termination condition is met: 3221) Calculate the gradient of the objective function: 3222) Update and optimize variables: 3223) Check termination conditions: If the conditions are met; 323) Return the final solution θ*; 33) Design a state sampling strategy to ensure that the Newton-Raphson iteration method does not result in no solution or even a trajectory that does not conform to the driving intention, and generate a more continuous trajectory cluster for subsequent filtering, as follows: In the vehicle coordinate system, a set of sampling points is generated as the endpoint state set of the trajectory; each sampling point contains state variables x, y, and θ, i.e., position and heading angle, thus forming the trajectory endpoint state set state{x, y, θ}; the starting state is set to {0, 0, 0}; however, to ensure the rationality of trajectory generation, the endpoint states need to be filtered and optimized, mainly in two stages: The first stage is the screening stage, which eliminates states that do not meet the vehicle kinematic constraints. The second stage is the optimization stage, which limits the range of change in trajectory curvature; 34) Trajectory optimization based on Lagrange interpolation; 341) For a trajectory, set the initial state of the vehicle to state{x} i y i θ i The final state is state{x}. target y target θ target }, {x i y i θ i }∈states, where states is the set of endpoint states generated by the sampling points; x i The x-coordinate of the initial state; y i θ represents the initial ordinate of the coordinate system; i The initial direction angle; x target The x-coordinate of the final state; y target θ represents the ordinate of the final state. target The direction angle of the final state; 342) Assume that the steering wheel control sequence generated from the starting point to the ending point is k(t), and k0 is the initial value of the sequence k(t). m Let k(t) be the median of the sequence k(t). f Let p be the final value of the k(t) sequence. When the reference vehicle speed is v, the time length of the steering wheel angle sequence is time = s / v, where s is the path length. At this point, let p = {k0, k...} m k f , s} are the key parameters; considering the smoothness of the steering wheel angle, Lagrange interpolation is used to calculate k0, k m k f The median value; Let the interpolation function be δ=f(t), and the interpolation coordinates be (0,0) and (time / 2,k). m (time,k) f ), where, according to the Lagrange interpolation formula: In the formula, i represents the interpolation order, and we take i = 2; L i (t) is the interpolation basis function: y i is the ordinate of the i-th interpolation; n is the total number of interpolations; In the formula, t represents time; j For the interpolation of node j; t i For the interpolation of node i; Substituting the data, we get: Substituting this into the quadratic Lagrange interpolation function, we have: Discretize the time, and obtain Among them, t k ∈[0,time]; Dividing the interval n into equal parts to obtain k, and using n=s / ds, we obtain the front wheel steering angle δ at each time point under the initial control parameters. k Then, based on the two-degree-of-freedom equations of the car, the state variables at each moment are derived. The state-space equations are: In the formula, x c y is the x-axis at time t = time; c θ represents the ordinate at time t = time; c The direction angle at time t = time; The state variable at time t = time is [x] c ,y c ,θ c The error between this point and the expected endpoint is: E=[target x -x c target y -y c target θ -θ c ]′ In the formula, target x The ideal x-coordinate; target y For target x The ideal ordinate; target θ For the ideal direction angle; Therefore, we obtain the control parameters s and k from the given control parameters s and k. m k f The mapping to the state variable error E at time t = time is denoted as: E=J(s,k m ,k f ) When the initial condition s = s i km = km i kf = kf i The time error is: E c =J(s,k m ,k f ) The gradient of the mapping function with respect to the independent variable is: In the formula, h is a constant; Let the Jacobian matrix be: According to the update formula of Newton's method, the gradient update direction is: dp=-J -1 ·E c Let the learning rate be α, then according to the update rule, update the control parameters {s km kf}: p = p + α·dp In the formula, p is the gradient; If the error E c If the value is less than the threshold, the update stops; the final control parameter p = {k0, k...} m k f Substituting 's' into the discrete two-degree-of-freedom state-space equations of the car yields the trajectory connecting the starting point to the specified endpoint.

5. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane-changing scenarios according to claim 1, characterized in that, The specific method for step four is as follows: 41) Conduct cruise sampling; In the cruise scenario, since there are no obstacles on the path, the vehicle plans its longitudinal trajectory according to the target cruise speed. In this case, it is not necessary to strictly specify the target position s1; it is only necessary to ensure that a reasonable trajectory is generated given the endpoint speed v1 and acceleration a1. The longitudinal motion trajectory of the vehicle is constructed using a quartic polynomial: s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 In the formula, s(t) is the trajectory; a0, a1, a2, a3 and a4 are constants; Assume the current state of the vehicle is s = s0. t is the sampling trajectory time, v i Let be the final velocity of the trajectory, and the boundary conditions are as follows: In the formula, s0 is the current position; v0 is the current velocity. Then the polynomial coefficients are solved, and the ST plot curve is determined. 42) Conduct on-vehicle sampling; At each sampling time point of the ST diagram, the end-point state is sampled, and the S value S is obtained at the rear foot of the obstacle. obs_rear_point A safety distance S is reserved below. safe_dis Then downwards at safe distances S sample_dis A point is sampled at a distance; since six boundary conditions need to be determined for the vehicle's initial state {s0, v0, a0} and terminal state {s1, v1, a1}, a fifth-degree polynomial is needed to construct the ST curve. The ST curve equation is as follows: s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 In the formula, a0, a1, a2, a3, a4, and a5 are constants; Assuming the vehicle in front has a speed of 25 km / h, and the vehicle's initial speed is 30 km / h with an initial acceleration of 0, the boundary conditions with a safe distance of 5m are set as follows: Substitute the boundary conditions into the ST curve equation to solve for the polynomial coefficients; 43) Conduct sampling while overtaking; The boundary conditions for trajectory planning are set as follows: Among them, S obs_front_point =26 is the position of the front heel of the vehicle in front, S overtake_front_dis =5 represents the safe overtaking distance; the overtaking sampling case has 6 boundary conditions, therefore a fifth-degree polynomial is constructed as follows: s(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 Substitute the boundary conditions into the fifth-degree polynomial to solve for the polynomial coefficients.

6. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane changing scenarios according to claim 1, characterized in that, The specific method for step five is as follows: 51) By establishing the resultant force equilibrium equation of the vehicle in the y-axis direction and the moment equilibrium equation about the center of mass, a two-degree-of-freedom dynamic model is established, as shown in the following formula: In the formula, k1 is the front wheel lateral stiffness; k2 is the rear wheel lateral stiffness; β is ××; u is longitudinal velocity; a is the distance from the front axle to the center of mass; b is the distance from the rear axle to the center of mass; δ is the steering angle; m is mass; ω is the lateral acceleration; r For yaw rate gain; I z Let Z be the moment of inertia of the vehicle about the Z-axis. By combining the two equations and eliminating v, the steady-state yaw rate gain can be obtained: Among them, w r yaw rate; L is the vehicle wheelbase; K is the understeer angle; u is the longitudinal speed; δ is the front wheel steering angle.

7. The intelligent vehicle lateral and longitudinal control method based on kinematic recursion in lane changing scenarios according to claim 6, characterized in that, The specific method for step six is ​​as follows: 61) In the vehicle coordinate system, the current pose of the vehicle is x=0, y=0, θ=0; In model predictive control, assuming an initial steering wheel angle is given, the vehicle will move to another position at the next moment. The pose at this time is in the vehicle coordinate system of the previous moment. The offset and heading angle at this time are calculated by the vehicle two-degree-of-freedom model. Assuming the vehicle speed u remains constant, integrating both sides of the steady-state yaw rate gain gives: In the formula, w r θ0 is the yaw rate gain; L is the wheelbase; K is the stability factor; u is the longitudinal velocity; θ0 is the initial heading angle. At this point, we obtain the state variable: heading angle θ; based on the small angle assumption, we have: If both sides travel at the same speed u, then: Integrating both sides, we get: At this point, the discrete state-space equation of the state variable x(k) = [θd]′ is obtained as follows: x(k+1)=Ax(k)+Bu(k)+C Where A = [1 1], 62) Predict state variables; The state variables from time k to k+Np are recursively calculated using the discretized state-space equations. The recursive process is as follows: In the formula, Np represents the prediction time domain; 63) Establishing a secondary planning problem; Based on the error between the predicted state variables and the planned trajectory, a quadratic programming problem is established to ultimately optimize the control input, enabling the vehicle to follow the planned trajectory. The formulas for calculating the heading angle error and lateral error are as follows: In the formula, k = 1, 2, ..., Np; Construct an objective function that includes lateral error and heading angle error; the objective function is in the form of a weighted sum of squared errors, aiming to minimize the error at all times in the prediction time domain; assume that the weighting coefficients of the errors are Q and R, where Q is the weight of the lateral error θ. err (k+i) are weighted, and R is applied to the heading angle error d. err Weigh (k+i) and let x(k) = [θ err (k),d err [k], the objective function J is expressed as: In the formula, x(k) is the state variable; u(k) is the control variable; The standard quadratic programming forms are as follows: In the formula, x is the optimization variable, i.e., the front wheel steering angle; H is the Hessian matrix; and g is a linear vector. Consider the vehicle's control input constraints; these constraints mainly include the steering wheel angle (i.e., the front wheel angle) and the rate of change of the steering wheel angle; assume the upper and lower limits of the steering wheel angle are δ. min and δ max The control input constraints are expressed as:

Images