Local trajectory planning method based on vehicle dynamics model

Abstract

The invention discloses a local trajectory planning method based on a vehicle dynamics model, and belongs to the technical field of automatic driving, and the method comprises the steps: S1, building a Frenet coordinate system and a conversion relation between the Frenet coordinate system and a Cartesian coordinate system; s2, establishing a three-degree-of-freedom vehicle dynamics model and a tire model used for real-time local trajectory planning, and setting a front wheel turning angle and a longitudinal speed of the vehicle as controlled variables; s3, performing local track description by adopting spline parameter control; s4, introducing a performance index functional, and performing quantitative evaluation on different control inputs so as to select an optimal control input; s5, generating a trajectory based on sequential quadratic programming; according to the local trajectory planning method based on the vehicle dynamics model, a solution taking physical feasibility and calculation efficiency into consideration is provided for real-time trajectory planning in a complex scene.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving, and in particular to a local trajectory planning method based on a vehicle dynamics model. Background Art

[0002] As an important development direction of intelligent transportation systems, autonomous driving technology has potential value in improving road safety and traffic efficiency, but its reliability and safety in practical applications still need to be systematically verified. In structured road scenarios, trajectory planning, as a key technical module, the accuracy and robustness of its algorithms directly affect the actual performance of autonomous driving systems, and there is still room for optimization. Aiming at problems such as infeasible trajectories caused by inconsistent models in traditional trajectory planning, the present invention proposes a trajectory planning framework based on vehicle dynamics, aiming to improve the feasibility of trajectories. Summary of the Invention

[0003] The purpose of the present invention is to provide a local trajectory planning method based on a vehicle dynamics model to solve the problems existing in the above background art.

[0004] To achieve the above purpose, the present invention provides a local trajectory planning method based on a vehicle dynamics model, including the following steps:

[0005] S1. Establish a Frenet coordinate system and its transformation relationship with the Cartesian coordinate system;

[0006] S2. Establish a three-degree-of-freedom vehicle dynamics model and a tire model for real-time local trajectory planning, and set the front wheel steering angle and longitudinal speed of the vehicle as control variables;

[0007] S3. Use spline parameter control for local trajectory description;

[0008] S4. Introduce a performance index functional to quantitatively evaluate different control inputs, and thus select the optimal control input;

[0009] S5. Generate a trajectory based on sequential quadratic programming.

[0010] Preferably, in step S1, the Frenet coordinate system is constructed based on the road reference line, its s-axis is the tangent direction of the reference line, the l-axis is the vertical direction, converting the curved path into a straight-line coordinate system. When the curve r(t) continuously changes in the Euclidean space, the arc length function s(t) is obtained by integrating the modulus of the curve derivative. Its strict monotonicity makes s and t form a bijective relationship, and then the curve is parameterized as the arc length function r(s). The transformation relationship is expressed as:

[0011]

[0012] Preferably, the vehicle dynamics model equation in step S2 is:

[0013]

[0014] where δ is the front wheel steering angle, u is the longitudinal speed, v is the lateral speed, ω is the yaw angular velocity, m is the vehicle mass, and I z is the moment of inertia of the vehicle about the z-axis, a is the distance from the center of mass to the front axle, b is the distance from the center of mass to the rear axle, and F yv is the lateral force of the front wheel, and F yh is the lateral force of the rear wheel, and F xv is the tangential force of the front wheel, and F xh is the tangential force of the rear wheel.

[0015] Preferably, the tire model in step S2 constructs a semi-empirical formula system through test data, which is:

[0016] F y = Dsin(Carctan(Bα - E(Bα - arctanBα)));

[0017] where F y is the tire lateral force, α is the tire sideslip angle, D represents the peak value, B controls the stiffness, C determines the curve shape, and E adjusts the curvature;

[0018] For the sideslip angles α v and α h of the front and rear tires of the vehicle, they are expressed as:

[0019]

[0020] Preferably, step S3 is specifically: parameterize the front wheel steering angle δ(t) and the longitudinal speed u(t) as control variables by varying with time using parameters p1 and p2 respectively:

[0021] u = [δ(p1, t), u(p2, t)] T ;

[0022] The front wheel steering angle δ(t) and the longitudinal speed u(t) are represented by a cubic spline curve composed of several polynomial segments. In the interval (t i , t i+1 ), δ(t) is expressed as:

[0023] δ i (t) = a i + b i (t - t i ) + c i (t - t i ) 2 + d i (t - t i )3 ;

[0024] Among them, the coefficients of each polynomial segment are determined by spline nodes that satisfy the C 1 continuity constraint, and the variation of the control quantity is positioned as:

[0025] p1 T = [δ1, δ2, δ3, δ4, …];

[0026] p2 T = [u1, u2, u3, u4, …];

[0027] Assume that the initial time is t0. At the same time, the final time t f is added to an additional parameter vector to establish a description of the motion trajectory of a three-degree-of-freedom vehicle dynamics model based on spline parameter control, as follows:

[0028] q = [p1 T , p2 T , t0, t f = [δ1, δ2, δ3, δ4, u1, u2, u3, u4, t0, t f T .

[0029] Preferably, the performance index functional in step S4 is expressed in the following form on the local trajectory:

[0030]

[0031] Among them, x represents the state quantity, and u represents the control quantity;

[0032] There are initial state constraints at the initial time t0:

[0033] x(t0) = x0;

[0034] At the final time t f there are terminal state constraints:

[0035] x(t f ) = x f ;

[0036] In addition, there are actual amplitude constraints or rate constraints on the control input quantity:

[0037] |u(t)| ≤ u max (t);

[0038]

[0039] Among them, u max (t) represents the maximum control quantity; represents the maximum control quantity change rate.​

[0040] Preferably, step S5 includes:

[0041] S51. Conduct constraint optimization considering the dynamic response;

[0042] S52. Pre-optimize various situations that occur in the offline stage to generate a look-up table of the optimal control parameter q, including: First, uniformly sample the terminal state constraints in the vehicle body coordinate system to cover all terminal states. Subsequently, for each sampling point, use the optimization algorithm to calculate the corresponding optimal trajectory and its control parameter, and finally generate the look-up table;

[0043] S53. Generate an optimal trajectory sequence based on the terminal state sampling.

[0044] Preferably, step S51 specifically includes:

[0045] Describe the local trajectory planning problem of the autonomous vehicle as:

[0046]

[0047] s.t. g(q) = x(q) - x f = 0;

[0048] And define the performance index functional J(q) as:

[0049]

[0050] where w1[δ(p1,t)] 2 characterizes the cumulative cost of the front wheel angle δ(t) in the input of the control quantity u; w2[u(p2,t) is for the longitudinal speed of the vehicle; both w1 and w2 are weight coefficients; u0 represents the initial state longitudinal speed; u f represents the target state longitudinal speed;

[0051] Before obtaining the feasible control quantities δ(t) and u(t), the control constraint conditions need to be applied to the spline curve in an implicit manner:

[0052]

[0053] where δ max represents the maximum front wheel angle; represents the front wheel angle change rate; represents the maximum front wheel angle change rate; u max represents the maximum longitudinal speed; represents the longitudinal acceleration; represents the maximum longitudinal acceleration;

[0054] For the control variables δ(t) and u(t), their initial state constraints and terminal state constraints are incorporated into the constraint condition system of the state variables:

[0055]

[0056] Among them, x0, y0, δ0, u0, respectively represent the initial horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, and longitudinal acceleration. x f , y f , δ f , u f , respectively represent the target horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, and longitudinal acceleration;

[0057] The KKT conditions of the optimization problem are transformed into the following matrix form:

[0058]

[0059] Among them, is the Hessian matrix of the objective function; is the transpose of the constraint gradient matrix; is the constraint gradient matrix; 0 is the zero matrix; Δq is the search direction of the state variables; Δλ is the update direction of the Lagrange multipliers; is the negative gradient of the Hamiltonian function; -g(q) is the negative value of the constraint function;

[0060] When calculating the Hessian matrix, the BFGS iterative algorithm is used to continuously update its estimate; after obtaining its optimal solution, the new iteration point q k+1 The update formula is:

[0061] q k+1 = q k + α k Δq;

[0062] Among them, α k represents the step size of the current iteration, and its value is determined by the Armijo criterion. The criterion form is:

[0063]

[0064] Among them, J(q) is the objective function, g(q) is the constraint function, and ||g(q)||1 is the 1-norm of the constraint function;

[0065] The Armijo criterion requires that α k satisfies the following inequality:

[0066]

[0067] Among them, the control parameter ρ ∈ (0, 1) is a constant; are the gradients of the objective function and the constraint function;

[0068] The stopping criterion of sequential quadratic programming is based on the following conditions:

[0069]

[0070] Among them, q k and λ k respectively represent the optimization variable and the Lagrange multiplier at the current iteration step, H(q k , λ k ) is the Hamiltonian function, g(q k ) is the constraint function, ‖·‖2 represents the Euclidean norm, and ε is a preset tolerance error threshold.

[0071] Preferably, step S53 specifically includes:

[0072] Before performing trajectory optimization, first determine the initial state of the vehicle within a given time interval and the desired target state Under different scenario types, the motion trajectory planning of the vehicle will be adjusted according to the terminal constraint state S of the predetermined target tar For each scenario, sample the entire terminal state according to the target terminal constraint state S tar For each time interval t ∈ [T i , T i+1 , perform spatial sampling of the start and end states of S(t), and the specific form is as follows:

[0073]

[0074] Among them, T i+1 represents the end state moment of this trajectory planning, s(T i+1 ) is the displacement of the end state, is the velocity of the end state, is the acceleration of the end state;

[0075] After the discretized sampling operation, the end state is updated to:

[0076]

[0077] Among them, Δs(T i+1 ) is the granularity of displacement sampling. By adjusting the values of Δs(T i+1 ) and T i+1 , further determine the entire trajectory set.

[0078] Therefore, the present invention adopts the above-mentioned local trajectory planning method based on a vehicle dynamics model, and has the following beneficial effects:

[0079] (1) A vehicle dynamics modeling framework for structured roads is constructed. By introducing the Frenet coordinate system to simplify the influence of road curvature on planning, a three-degree-of-freedom vehicle dynamics model is established and the magic formula tire model is used to characterize the lateral force. An innovative cubic spline curve parameterization method is used to discretize the control variables, realizing the deep embedding of vehicle dynamics constraints in the trajectory generation stage, and providing a high-precision mathematical model basis for subsequent planning and control links;

[0080] (2) A local trajectory planning method under dynamic constraints is proposed; in the local trajectory generation stage, a sequential quadratic programming algorithm is used to solve the feasible trajectory cluster that satisfies the terminal state constraints. By designing a composite cost function that fuses the acceleration change rate, lateral deviation, and speed tracking error, and cooperating with a safety margin evaluation mechanism to achieve optimal trajectory screening. An offline pre-optimization strategy is introduced, and a real-time trajectory query database is pre-computed based on typical working conditions to improve the real-time planning ability in complex scenarios.

[0081] Next, through the drawings and embodiments, the technical solutions of the present invention will be further described in detail. Description of the Drawings

[0082] Figure 1 is the flow chart of the present invention;

[0083] Figure 2 is an example diagram of the Frenet-Cartesian coordinate transformation of the present invention;

[0084] Figure 3 is a schematic diagram of the transformation relationship between the Frenet and global coordinate systems of the present invention;

[0085] Figure 4 is a schematic diagram of the three-degree-of-freedom single-track vehicle dynamics model of the present invention;

[0086] Figure 5 is a schematic diagram of the spline parameterization of the front wheel steering angle δ(t) of the present invention;

[0087] Figure 6 is a schematic diagram of the vehicle motion trajectory under the input of δ(t) and u(t) of the present invention, where (a) is the change of the front wheel steering angle δ with time t, (b) is the change of the longitudinal speed u with time t, and (c) is the motion trajectory of the vehicle under the control of δ and u;

[0088] Figure 7 is a schematic diagram of the optimization control result and trajectory when the SQP iteration is completed in the lane change scenario of the present invention;

[0089] Figure 8Schematic diagram of the process of generating a lookup table for the initial state of the vehicle and various terminal states of the present invention;

[0090] Figure 9 Schematic diagram of the lane change trajectory curve in the straight road lane change scenario of the embodiment of the present invention;

[0091] Figure 10 Schematic diagram of key state parameters during the vehicle movement in the straight road lane change scenario of the embodiment of the present invention, where (a) is the front wheel steering angle, (b) is the yaw rate, (c) is the longitudinal speed, (d) is the longitudinal acceleration, (e) is the lateral speed, (f) is the lateral acceleration, (g) is the heading angle, and (h) is the trajectory curvature;

[0092] Figure 11 Schematic diagram of the lane change trajectory curve in the curved road lane change scenario of the embodiment of the present invention;

[0093] Figure 12 Schematic diagram of key state parameters during the vehicle movement in the curved road lane change scenario of the embodiment of the present invention, where (a) is the front wheel steering angle, (b) is the yaw rate, (c) is the longitudinal speed, (d) is the longitudinal acceleration, (e) is the lateral speed, (f) is the lateral acceleration, (g) is the heading angle, and (h) is the trajectory curvature;

[0094] Figure 13 Schematic diagram of the vehicle trajectory curve in the straight road overtaking scenario of the embodiment of the present invention;

[0095] Figure 14 Schematic diagram of key state parameters during the vehicle movement in the straight road overtaking scenario of the embodiment of the present invention, where (a) is the front wheel steering angle, (b) is the yaw rate, (c) is the longitudinal speed, (d) is the longitudinal acceleration, (e) is the lateral speed, (f) is the lateral acceleration, (g) is the heading angle, and (h) is the trajectory curvature;

[0096] Figure 15 Schematic diagram of the vehicle trajectory curve in the curved road overtaking scenario of the embodiment of the present invention;

[0097] Figure 16 Schematic diagram of key state parameters during the vehicle movement in the curved road overtaking scenario of the embodiment of the present invention, where (a) is the front wheel steering angle, (b) is the yaw rate, (c) is the longitudinal speed, (d) is the longitudinal acceleration, (e) is the lateral speed, (f) is the lateral acceleration, (g) is the heading angle, and (h) is the trajectory curvature. Detailed implementation manners

[0098] The following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention.

[0099] See also Figure 1 , a local trajectory planning method based on a vehicle dynamics model, comprising the following steps:

[0100] S1. Establish the Frenet coordinate system and its conversion relationship with the Cartesian coordinate system;

[0101] The Frenet coordinate system is constructed based on the road reference line, with the s axis being the tangent direction of the reference line and the l axis being the vertical direction. Figure 2 As shown in Figure 2, the curved path is converted into a straight coordinate system to simplify path planning. When the curve r(t) changes continuously in the Euclidean space, the arc length function s(t) is obtained by integrating the modulus of the curve derivative. Its strict monotonicity makes s and t form a bijective relationship, and then the curve is parameterized into the arc length function r(s). The conversion relationship is expressed as:

[0102]

[0103] like Figure 3 As shown, the relationship between Frenet and Cartesian coordinate conversion is shown, where the symbols mean: A point on the trajectory; are the tangent vector and normal vector of the trajectory point respectively; is the angle between the tangent vector of the trajectory point and the x-axis; Reference line projection point; Tangent vector and normal vector of the projection point; The angle between the tangent vector of the projection point and the x-axis; l is the distance between the trajectory point and the projection point; v x , a x , κ x are the velocity, acceleration and curvature of the trajectory points respectively; are the longitudinal velocity and acceleration in Frenet coordinates respectively; are the lateral velocity and acceleration in Frenet coordinates respectively; Δθ is the heading angle deviation.

[0104] The final Cartesian coordinate to Frenet coordinate relationship is as follows:

[0105]

[0106] The relationship between Frenet coordinates and Cartesian coordinates is as follows:

[0107]

[0108] Among them, where x r ,y r ,v r ,a r ,κ rThey are the horizontal and vertical coordinates, speed, acceleration, and trajectory curvature in the Frenet coordinate system, x x , y x , v x , a x , κ x They are the coordinates, speed, acceleration, and trajectory curvature in the Cartesian coordinate system. l′ and l” are the first and second derivatives of l with respect to s.

[0109] S2. Establish a three - degree - of - freedom vehicle dynamics model and a tire model for real - time local trajectory planning, and set the front - wheel steering angle and longitudinal speed of the vehicle as control variables;

[0110] To balance model accuracy and complexity and adapt to the requirements of trajectory planning, the following assumptions are established:

[0111] (1) The front - wheel steering angle is used as the direct control input, ignoring the dynamics of the steering system;

[0112] (2) The vehicle is regarded as a rigid body, excluding suspension deformation and three - dimensional space motion;

[0113] (3) A single - track model is adopted, ignoring lateral / longitudinal load transfer and centroid offset;

[0114] (4) The vehicle is restricted to move in a two - dimensional plane, ignoring vertical vibration and slope effects.

[0115] Construct a three - degree - of - freedom dynamics model including longitudinal along the x - axis, lateral along the y - axis, and yaw along the z - axis, as Figure 4 shown.

[0116] The vehicle dynamics model equation is:

[0117]

[0118] Among them, δ is the front - wheel steering angle, u is the longitudinal speed, v is the lateral speed, ω is the yaw angular velocity, m is the vehicle mass, I z is the moment of inertia of the vehicle about the z - axis, a is the distance from the centroid to the front axle, b is the distance from the centroid to the rear axle, F yv is the lateral force of the front wheel, F yh is the lateral force of the rear wheel, F xv is the tangential force of the front wheel, F xh is the tangential force of the rear wheel.

[0119] The tire model constructs a semi - empirical formula system through experimental data, which is:

[0120] F y =Dsin(Carctan(Bα - E(Bα - arctanBα))) (6)

[0121] Among them, Fy where $F_y$ is the lateral force of the tire, $\alpha$ is the slip angle of the tire, $D$ represents the peak value, $B$ controls the stiffness, $C$ determines the curve shape, and $E$ adjusts the curvature; the values of the parameters $B$, $C$, $D$, and $E$ are shown in Table 1:

[0122] Table 1 Magic formula tire model parameters

[0123] Tire B C D E Front wheel 6.7653 1.3 6436 -1.999 Rear wheel 9.0023 1.3 5430 -1.999

[0124] For the slip angles $\alpha_{f}$ v and $\alpha_{r}$ h of the front and rear tires of the vehicle, they are expressed as:

[0125]

[0126] S3. Local trajectory description is performed using spline parameter control, specifically:

[0127] The front wheel steering angle $\delta(t)$ and the longitudinal speed $u(t)$ used as control variables are parameterized over time using the parameters $p1$ and $p2$ respectively:

[0128] $u = [\delta(p1,t), u(p2,t)]$ T (8)

[0129] Although this way of parameterizing the control variables may limit the search range of the control variables to a subspace within all feasible regions, by reasonably choosing the parameterization method, almost all feasible control situations applicable to the vehicle motion can be described. In this embodiment, the front wheel steering angle $\delta(t)$ and the longitudinal speed $u(t)$ of the control variables are represented by a cubic spline curve composed of several polynomial segments. In the interval $(t$ i , $t$ i+1 ), $\delta(t)$ is expressed as:

[0130] $\delta$ i (t) = a i + b i (t - t i ) + c i (t - t i ) 2 + d i (t - t i ) 3 (9)

[0131] where the coefficients of each polynomial segment are determined by spline knots that satisfy the $C$ 1 continuity constraint, $C$ 1The continuity constraint ensures that adjacent polynomial segments not only have equal function values but also equal first-order derivatives at the connection points. This makes the entire spline curve globally smooth, avoiding sudden changes and discontinuities. This means that the shape of the spline curve (i.e., the variation of the control variables) can be defined by a small number of spline nodes, as Figure 5 shown, the variation of the control variables is located as:

[0132] p1 T =[δ1,δ2,δ3,δ4,…] (10)

[0133] p2 T =[u1,u2,u3,u4,…] (11)

[0134] As mentioned before, the control variables of the front-wheel steering angle δ(t) and the longitudinal velocity u(t) are parameterized as cubic splines (10) and (11) with uniformly distributed spline nodes and arc lengths. The number of spline nodes p is chosen to be 4 because it is usually sufficient to describe all possibilities within a relatively short planning range. To simplify the problem, assume the initial time is t0, and at the same time, add the final time t f to an additional parameter vector to establish a description of the motion trajectory of the three-degree-of-freedom vehicle dynamics model based on spline parameter control, as follows:

[0135] q = [p1 T ,p2 T ,t0,t f =[δ1,δ2,δ3,δ4,u1,u2,u3,u4,t0,t f T (12).

[0136] According to the above content, a discretized three-degree-of-freedom vehicle dynamics model based on spline parameter control is established in Matlab. Considering the requirements of vehicle dynamics research and real-time performance for autonomous driving, the model update frequency is set to 100Hz to capture vehicle dynamic changes in a timely manner and provide effective data for subsequent analysis and decision-making. The physical parameters of the vehicle model are shown in Table 2.

[0137] Table 2 Physical parameters of the vehicle model

[0138] Parameter Value Unit a 1.615 m b 1.295 m m 1270 kg <![CDATA[I z > 1536.7 <![CDATA[kg·m 2 > g 9.8 <![CDATA[kg·m / s 2 >

[0139] In the three-degree-of-freedom vehicle dynamics model constructed based on the Matlab platform in this embodiment, the front-wheel steering angle and the longitudinal velocity are set as the input variables of the model. When a specific set of front-wheel steering angles and longitudinal velocities are given as inputs, the model can calculate the motion state and driving trajectory of the vehicle over a period of time, as Figure 6 shown. ​

[0140] The results show that under the continuous input of δ(t) and u(t), the vehicle model exhibits good stability and can operate continuously and stably. Moreover, during the process of 2000 state updates, the average computing time of the model is about 12 ms, and the computing speed meets the requirements of the subsequent real-time trajectory planning task for time performance. In addition, at any moment during the vehicle movement, various other state variables, such as acceleration and yaw rate, can also be calculated through this model.

[0141] S4. Introduce the performance index functional to quantitatively evaluate different control inputs, so as to select the optimal control input;

[0142] The core of the local trajectory generation problem of autonomous vehicles lies in finding a reasonable control variable u. In the problem of local trajectory planning of autonomous vehicles, the set of control input quantities that meet the terminal target state requirements is not unique. To screen out the optimal control input from numerous possible candidate control input sets, a performance index functional needs to be introduced. This performance index functional can quantitatively evaluate different control inputs according to specific evaluation criteria, thus providing an effective basis for selecting the optimal control input. In this way, while ensuring that the vehicle reaches the terminal target state, performance optimization goals such as the shortest driving time and the smoothest driving path can be achieved. The performance index functional is expressed in the following form on the local trajectory:

[0143]

[0144] where x represents the state quantity and u represents the control quantity;

[0145] There are initial state constraints at the initial moment t0:

[0146] x(t0) = x0 (14)

[0147] At the final moment t f There are terminal state constraints:

[0148] x(t f ) = x f (15)

[0149] In addition, there may be actual amplitude constraints (limiting the value range of the control variable) or rate constraints (limiting the change rate of the control variable) for the control input quantity:

[0150] |u(t)| ≤ u max (t) (16)

[0151]

[0152] where u max(t) represents the maximum control amount; represents the maximum control amount change rate.

[0153] S5. Generate a trajectory based on sequential quadratic programming. In the above content, the control amount u has been parameterized as q, and the above optimal control problem can accordingly be transformed into a constrained optimization problem with the free parameter q as the variable. It includes:

[0154] S51. Consider the dynamic response for constrained optimization;

[0155] Describe the local trajectory planning problem of the autonomous vehicle as:

[0156]

[0157] s.t. g(q) = x(q) - x f = 0 (19)

[0158] During the local trajectory planning process of the autonomous vehicle, the performance index functional J(q) is used to characterize the target to be minimized, and is defined as:

[0159]

[0160] where, w1[δ(p1,t)] 2 characterizes the cumulative cost of the front wheel angle δ(t) in the control amount u input. It helps to control the steering of the autonomous vehicle to reach a stable state as soon as possible. Among the many candidate trajectory control inputs that satisfy the terminal state constraints, the smaller the value of this integral, the smaller the yaw rate, which means the higher the stability of the vehicle during the local trajectory movement, and thus it can drive more smoothly; w2[u(p2,t)] is for the longitudinal speed of the vehicle, which helps to quickly stabilize the longitudinal speed of the vehicle at the desired speed value, reduce the acceleration, and improve the comfort; w1 and w2 are both weight coefficients; u0 represents the initial state longitudinal speed; u f represents the target state longitudinal speed; therefore, this performance index functional can make a reasonable trade-off between fast steering stability and driving comfort. Among them, the adjustment of the trade-off is achieved through the weight coefficients w1 and w2. By adjusting the values of w1 and w2, the emphasis on steering stability and speed stability can be changed, so as to optimize the local trajectory planning of the vehicle in different driving scenarios to meet different performance requirements and driving experience expectations.

[0161] Before obtaining the feasible control amounts δ(t) and u(t), the control constraint conditions (such as amplitude limit and rate limit) need to be applied to the spline curve in an implicit way:

[0162]

[0163] where, δmax Denotes the maximum front wheel steering angle; Denotes the front wheel steering angle change rate; Denotes the maximum front wheel steering angle change rate; u max Denotes the maximum longitudinal speed; Denotes the longitudinal acceleration; Denotes the maximum longitudinal acceleration;

[0164] To ensure the accuracy and feasibility requirements of the planning results, for the control variables δ(t) and u(t), their initial state constraints and terminal state constraints are incorporated into the constraint condition system of the state variables:

[0165]

[0166] Among them, x0, y0, δ0, u0, respectively represent the initial horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, longitudinal acceleration, x f , y f , δ f , u f , respectively represent the target horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, longitudinal acceleration.

[0167] The Sequential Quadratic Programming (SQP) algorithm, as an effective numerical calculation method for dealing with nonlinear constraint optimization problems, has significant advantages in the field of trajectory planning. It approximates the solution of the original problem through serialized quadratic programming sub-problems, and can effectively handle equality and inequality constraint conditions while ensuring convergence. The application of this method in autonomous driving trajectory planning is mainly reflected in the balance between its ability to handle complex constraint conditions and computational efficiency. Based on the theoretical framework of constraint optimization, the mathematical description of this problem can be achieved by constructing the Hamiltonian function. In the context of constraint optimization, this function can also be expressed as the Lagrangian function, and its standard mathematical expression can be defined as:

[0168] H(q, λ) = J(q) + λ T g(q) (24)

[0169] Among them, λ is the Lagrange multiplier;

[0170] To solve for the optimal control input, it is necessary to derive the first-order necessary conditions of the Hamiltonian function based on the variational principle, and its mathematical expression is as follows:

[0171]

[0172] The gradient of the constraint function with respect to the state variable q can be expressed as:

[0173]

[0174] where x(q) represents the system state function, and x f represents the desired terminal state, δ f i and u i correspond to the control inputs.

[0175] In the mathematical derivation of the SQP algorithm, performing a Taylor expansion of the Hamiltonian function is a key step in constructing a local approximation model. Performing a second-order Taylor expansion on Equation (24) at the iteration point (q, λ) and truncating the higher-order infinitesimals yields the linearized equation:

[0176]

[0177] The establishment of this system of equations embodies the core idea of the SQP method - transforming a nonlinear optimization problem into a sequence of quadratic programming problems through local linearization. To determine an effective search direction and improve the algorithm's convergence efficiency, it is necessary to accurately calculate the Hessian matrix of the Hamiltonian function:

[0178]

[0179] When constructing the Hessian matrix, the second-order optimality of the KKT conditions should be strictly followed to ensure the accuracy of the optimal solution; the geometric characteristics of the original problem are retained through local approximation to effectively reflect the structure of the original problem; the compatibility between the sub-problem and the original problem is ensured to avoid deviations between the approximate solution and the true solution.

[0180] The second term in Equation (30) represents the weighted combination of the Lagrange multiplier λ and the second derivative of the constraint function, reflecting the influence of the constraint conditions on the optimization problem. Specifically, λ represents the contribution of the constraint g i (q) to the system energy, and the change in the weight directly determines the importance of the constraint. This content combines the curvature information of the constraint with the convexity characteristics of the objective function, and can more accurately reflect the nature of the local optimization problem.

[0181] Transform the KKT conditions of the optimization problem into the following matrix form:

[0182]

[0183] where, is the Hessian matrix of the objective function; is the transpose of the constraint gradient matrix; is the constraint gradient matrix; 0 is the zero matrix; Δq is the search direction of the state variable; Δλ is the update direction of the Lagrange multiplier; is the negative gradient of the Hamiltonian function; -g(q) is the negative value of the constraint function;

[0184] In the above equations, the Jacobian matrices and cannot be directly obtained by analytical methods, so numerical methods need to be used for estimation. To linearize the motion equations, we use the forward difference or central difference method to estimate these partial derivatives. Specifically, the numerical approximation form of the partial derivatives can be expressed in the following form:

[0185] 1. Forward difference method:

[0186]

[0187] 2. Central difference method:

[0188]

[0189] When calculating the Hessian matrix, the BFGS iterative algorithm is used to continuously update its estimate; the specific update formula is as follows:

[0190]

[0191] where s k and y k are defined as:

[0192] s k = q k+1 - q k (35)

[0193]

[0194] In the BFGS algorithm, the initial Hessian matrix H0 is usually set to the identity matrix. However, to accelerate convergence and reduce the number of iterations, a more appropriate initial matrix can also be selected by numerically approximating the second derivative of the Hamiltonian function. Specifically, the initial Hessian matrix can be estimated by the following formula:

[0195]

[0196] where e is a small increment, H i,j (q,λ) represents the second derivative of the Hamiltonian function at the point (q,λ), q m is the m-th component of q, q n is the n-th component of q, q0 is the initial value of q, and λ0 is the initial value of λ.

[0197] After obtaining its optimal solution, the new iteration point q k+1 The update formula is:

[0198] q k+1 = q k + α k Δq (38)

[0199] where α k represents the step size of the current iteration, and its value is determined by the Armijo criterion, and the criterion form is:

[0200]

[0201] where J(q) is the objective function, g(q) is the constraint function, and ||g(q)||1 is the 1-norm of the constraint function;

[0202] The Armijo criterion requires that α k satisfies the following inequality:

[0203]

[0204] where the control parameter ρ ∈ (0, 1) is a constant; are the gradients of the objective function and the constraint function;

[0205] When the SQP iteration process satisfies the first-order necessary condition or reaches the set maximum number of iterations, the iteration process will stop and output the optimization result q. Specifically, the stopping criterion is based on the following conditions:

[0206]

[0207] where q k and λ k respectively represent the optimization variable and the Lagrange multiplier of the current iteration step, H(q k , λ k ) is the Hamiltonian function, g(q k ) is the constraint function, ||·||2 represents the Euclidean norm, and ε is the preset tolerance error threshold.

[0208] Figure 7Shows the control parameters optimized by SQP iteration and the lane change trajectory planning results during the lane change operation. Above the figure, the curves of the control input - front wheel angle δ and the vehicle longitudinal speed u changing with time t are presented respectively, and the dots represent the optimized spline parameter control points. Below is the lane change trajectory diagram of the vehicle, where the curve represents the driving trajectory of the vehicle, the dots mark the terminal target state position (x, y) = [40.000, 3.750] after the vehicle completes the lane change operation, and the horizontal dashed line represents the center line of the lane.

[0209] S52. Pre-optimize various situations in the offline stage to generate a lookup table of the optimal control parameter q, including: First, in the body coordinate system, uniformly sample the terminal state constraints to cover all terminal states. Subsequently, for each sampling point, use the optimization algorithm to calculate the corresponding optimal trajectory and its control parameters, and finally generate the lookup table, as Figure 8 shown.

[0210] S53. Generate an optimal trajectory sequence based on the terminal state sampling.

[0211] Before performing trajectory optimization, first determine the initial state of the vehicle within a given time interval and the desired target state The initial state S0 is usually known, while the target state S tar and the planning time T may change according to the vehicle's behavior requirements. Under different scenario types, the vehicle's motion trajectory planning will be adjusted according to the terminal constraint state S tar of the predetermined target. In each scenario, sample the entire terminal state according to the target terminal constraint state S tar Perform spatial sampling of the start and end states of S(t) within each time interval t ∈ [T i , T i+1 , and the specific form is as follows:

[0212]

[0213] where, T i+1 represents the end state moment of this trajectory planning, s(T i+1 ) is the displacement of the end state, is the speed of the end state, is the acceleration of the end state;

[0214] After the discretized sampling operation, the end state is updated to:

[0215]

[0216] where, Δs(Ti+1 ) is the granularity of displacement sampling. By adjusting Δs(T i+1 ) and T i+1 , the entire trajectory set is further determined.

[0217] To verify the performance of this method, virtual simulation tests in multiple typical scenarios were designed and implemented in Matlab in this embodiment, including tests of typical driving scenarios such as lane-changing scenarios and overtaking scenarios on both straight roads and curved roads. The basic parameters of the vehicle used in the tests are shown in Table f = 4 s, and the planning information is updated once per second. The definition of the weight matrix Q of each weight coefficient in the trajectory planning algorithm is as follows:

[0218] Q = [w1, w2, w3, w4, w5, w6] = [1, 0.1, 0.5, 0.5, 1, 1] (44)

[0219] For comparison, a traditional sampling-based quintic polynomial trajectory planning method (hereinafter referred to as the quintic method) was adopted in this embodiment. This method does not consider the vehicle's dynamic model during the trajectory planning process. By comparing the planning effects of the two methods, the advantages of the proposed method in trajectory generation are further analyzed.

[0220] Table 3 Peak state of each test scenario index

[0221]

[0222] Note: The units of each index are °, ° / s, m / s2, m / s2, °, 1 / m respectively. Method 1 is this method, and Method 2 is the quintic method.

[0223] 1) Straight road condition

[0224] In the experimental setup, the road width follows the national standard and is 3.75 m. The desired driving speed of the autonomous vehicle is 10 m / s. The start time of the vehicle's lane change is 1.5 s, the end time is 5.5 s, and the duration is 4 s. During this process, the vehicle travels approximately 40 m along the lane centerline and completes a lane change operation with a lateral displacement of 3.75 m. After the lane change is completed, the vehicle continues to travel along the lane centerline of the lane after the lane change at the desired speed.

[0225] From Figure 9 the trajectory diagram shown, it can be seen that during the lane change process, the vehicle's speed remains basically stable. In particular, the trajectory planning method proposed in this paper is superior to the quintic method in terms of path smoothness. During the lane change operation, the maximum front wheel angle of the vehicle is significantly smaller than that of the quintic method, which is shown in Figure 10(a) is verified, indicating that this method is more stable during lane - changing operations and reduces the excessive requirements for vehicle control.

[0226] In addition, Figure 10 (b), (g) and (h) show the changes in yaw rate, heading angle and trajectory curvature of the vehicle during lane - changing. Compared with the quintic method, the proposed planning scheme in this paper significantly reduces the fluctuation amplitude of these key dynamic parameters, indicating that the stability and smoothness of the path have been significantly improved.

[0227] 2) Curved - road condition

[0228] Figure 11 and Figure 12 show the simulation test results of the lane - changing scenario on a curved road, where the radius of curvature of the road is 300 m. In this test, the desired cruising speed of the vehicle is set to 12 m / s. The test results are similar to those of the straight - lane - changing scenario, and the speed of the vehicle remains stable throughout the lane - changing process. In this test, the lane - changing operation of the vehicle lasts about 4 s and then continues to drive along the curve. After the lane - changing is completed, the steering angle of the vehicle stabilizes at about 0.51°, and the yaw rate stabilizes at 2.29° / s.

[0229] As Figure 12 shown, the peak values of the vehicle using the quintic method in terms of lateral speed ( Figure 12 (e)) and lateral acceleration ( Figure 12 (f)) are slightly larger than those of the trajectory - planning method proposed in this paper. This indicates that the quintic method inadequately considers the dynamic constraints of the vehicle during trajectory planning, resulting in higher lateral speed and acceleration, which may affect the comfort and stability of the vehicle. The method proposed in this paper significantly reduces the fluctuation of lateral dynamics through a smoother path planning, ensuring a more stable movement of the vehicle.

[0230] In addition, the changes in the heading angle ( Figure 12 (g)) and trajectory curvature ( Figure 12 (h)) of the trajectory - planning method proposed in this paper are also smaller, further indicating that this method pays more attention to the smoothness and stability of the path during the planning process.

[0231] 3) Overtaking scenario

[0232] 3.1) Overtaking scenario on a straight road

[0233] Figure 13 and Figure 14Shows the test results in the scenario of overtaking on a straight road. In these figures, the green rectangular boxes represent the positions of the autonomous vehicles, while the red rectangular boxes represent the positions of the obstacle vehicles ahead. It should be noted that the vehicle borders in the figures overlap, but this does not mean that a collision has occurred, because these overlapping areas do not represent the positions of the vehicles at the same moment, and the time factor is not considered in the figures. Therefore, no actual collision has occurred. The obstacle vehicle represented by the red border travels at a constant speed from the starting position of 35m to 75m during the test. When the autonomous vehicle detects that the vehicle ahead is about 20m away from itself at 1s, it starts the lane-changing overtaking operation.

[0234] During the entire overtaking process, the speed of the autonomous vehicle accelerates from the initial 10m / s to 16m / s, then completes the overtaking and resumes uniform motion. As Figure 14 (c) and (e) show, the changes in longitudinal speed and lateral speed are relatively smooth. In terms of longitudinal acceleration, the method proposed in this paper reaches a peak value of 1.81m / s at 2.9 seconds 2 , while the peak value of longitudinal acceleration under the quintic method control is 2.51m / s 2 . It can be seen that the quintic method has a faster acceleration response during overtaking and uses larger front wheel angles and yaw angular velocities during the high-speed driving stage. In contrast, the acceleration peak value under the trajectory planning method proposed in this paper is smaller, indicating that this method can control the vehicle motion more smoothly throughout the overtaking process, avoiding sharp acceleration and steering, thus ensuring better vehicle dynamics performance. The results show that the method proposed in this paper optimizes the local motion trajectory by fully considering the vehicle's dynamic response during the trajectory planning stage, thereby effectively improving the vehicle stability and comfort during overtaking.

[0235] 3.2) Overtaking scenario on a curved road

[0236] Figure 15 and Figure 16 show the test results in the overtaking scenario on a curved road. In the curved road overtaking test, the local trajectory planning algorithm proposed in this paper can still complete the overtaking task efficiently and smoothly, and the overall performance is better than that of the quintic method.

[0237] Specifically, the vehicle maintains a small corner change during overtaking, reduces the fluctuation of yaw angular velocity, and ensures vehicle stability. In terms of longitudinal acceleration and lateral acceleration, the method proposed in this paper also successfully controls the acceleration amplitude of the vehicle, avoids excessive lateral and longitudinal forces, thereby effectively reducing the impact on the vehicle, and improving the overall comfort and safety.

[0238] These results further verify the strong adaptability of the local trajectory planning algorithm proposed in this paper in complex driving environments. Especially when considering the vehicle dynamics model, it can significantly improve the performance of the autonomous driving system in dynamic driving scenarios compared to the quintic method. Whether on straight roads or curved roads, the proposed algorithm demonstrates excellent performance, further proving its feasibility and advantages in practical applications.

[0239] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that they can still modify or equivalently replace the technical solutions of the present invention, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A local trajectory planning method based on a vehicle dynamics model, characterized in that, It includes the following steps: S1. Establish a Frenet coordinate system and the conversion relationship with the Cartesian coordinate system; S2. Establish a three-degree-of-freedom vehicle dynamics model and a tire model for real-time local trajectory planning, and set the front wheel steering angle and longitudinal speed of the vehicle as control variables; S3. Use spline parameter control for local trajectory description; S4. Introduce a performance index functional to quantitatively evaluate different control inputs, so as to select the optimal control input; S5. Generate a trajectory based on sequential quadratic programming.

2. The local trajectory planning method based on a vehicle dynamics model according to claim 1, characterized in that In step S1, the Frenet coordinate system is constructed based on the road reference line. Its s-axis is the tangent direction of the reference line, and the l-axis is the vertical direction, converting the curved path into a straight-line coordinate system. When the curve r(t) continuously changes in the Euclidean space, the arc length function s(t) is obtained by integrating the modulus of the curve derivative. Its strict monotonicity makes s and t form a bijective relationship, and then the curve is parameterized as the arc length function r(s). The conversion relationship is expressed as:

3. A local trajectory planning method based on a vehicle dynamics model according to claim 1, characterized in that, The vehicle dynamics model equation in step S2 is: where δ is the front wheel steering angle, u is the longitudinal velocity, v is the lateral velocity, ω is the yaw rate, m is the vehicle mass, I z is the moment of inertia of the vehicle about the z-axis, a is the distance from the center of mass to the front axle, b is the distance from the center of mass to the rear axle, F yv is the lateral force of the front wheel, F yh is the lateral force of the rear wheel, F xv is the tangential force of the front wheel, F xh is the tangential force of the rear wheel.

4. The local trajectory planning method based on a vehicle dynamics model according to claim 3, characterized in that, The tire model in step S2 constructs a semi-empirical formula system through experimental data, which is: F y = D sin(C arctan(Bα - E(Bα - arctan Bα))); Among them, F y is the lateral force of the tire, α is the tire slip angle, D characterizes the peak value, B controls the stiffness, C determines the curve shape, and E adjusts the curvature; For the sideslip angles α v and α h of the front and rear tires of the vehicle, it is expressed as:

5. A local trajectory planning method based on a vehicle dynamics model according to claim 4, characterized in that, Step S3 is specifically: The front wheel steering angle δ(t) and longitudinal speed u(t) as control variables are respectively parameterized with time variation by means of parameters p1 and p2: u = [δ(p1, t), u(p2, t)] T ; The front wheel steering angle δ(t) and the longitudinal speed u(t) are represented by a cubic spline curve composed of several polynomial segments. In the interval (t i , t i+1 ), δ(t) is expressed as: δ i (t) = a i + b i (t - t i ) + c i (t - t i ) 2 + d i (t - t i ) 3 ; Among them, the coefficients of each polynomial segment are determined by spline nodes that satisfy the C 1 continuity constraint, and the variation of the control quantity is positioned as follows: p1 T = [δ1, δ2, δ3, δ4, …]; p2 T = [u1, u2, u3, u4, …]; Assume that the initial time is t0, and at the same time, add the final time t f to an additional parameter vector, and establish a description of the motion trajectory of a three-degree-of-freedom vehicle dynamics model based on spline parameter control as follows: q = [p1 T , p2 T , t0, t f = [δ1, δ2, δ3, δ4, u1, u2, u3, u4, t0, t f T 。​ 6. The local trajectory planning method based on a vehicle dynamics model according to claim 5, characterized in that, The performance index functional in step S4 is expressed in the following form on the local trajectory: where x represents the state variable and u represents the control variable; At the initial moment t0, there are initial state constraints: x(t0) = x0; At the final moment t f There are terminal state constraints: x(t f ) = x f ; In addition, there are actual amplitude constraints or rate constraints on the control input quantity: |u(t)| ≤ u max (t); Among them, u max (t) represents the maximum control amount; represents the maximum control amount change rate.

7. A local trajectory planning method based on a vehicle dynamics model according to claim 6, characterized in that, Step S5 includes: S51. Consider the dynamic response for constraint optimization; S52. Perform pre-optimization for various situations in the offline stage, so as to generate a look-up table of the optimal control parameter q, including: First, under the body coordinate system, uniformly sample the terminal state constraints to cover all terminal states. Subsequently, for each sampling point, use the optimization algorithm to calculate the corresponding optimal trajectory and its control parameters, and finally generate the look-up table; S53. Generate an optimal trajectory sequence based on the terminal state sampling.

8. A local trajectory planning method based on a vehicle dynamics model according to claim 7, characterized in that, Step S51 specifically includes: Describe the local trajectory planning problem of the autonomous vehicle as: such that \(g(q)=x(q)-x\) f = 0; And define the performance index functional J(q) as: where, w1[δ(p1,t)] 2 represents the cumulative cost of the front wheel angle δ(t) in the input of the control quantity u; w2[u(p2,t) is for the longitudinal speed of the vehicle; both w1 and w2 are weight coefficients; u0 represents the longitudinal speed in the initial state; u f represents the longitudinal speed in the target state; Before obtaining the feasible control variables δ(t) and u(t), the control constraint conditions need to be applied to the spline curve in an implicit manner: where, δ max represents the maximum front wheel steering angle; represents the front wheel steering angle change rate; represents the maximum front wheel steering angle change rate; u max represents the maximum longitudinal speed; represents the longitudinal acceleration; represents the maximum longitudinal acceleration; For the control variables δ(t) and u(t), their initial state constraints and terminal state constraints are incorporated into the constraint condition system of the state variables: where x0, y0, δ0, u0, represent the initial horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, and longitudinal acceleration respectively, and x f , y f , δ f , u f , represent the target horizontal and vertical coordinates, heading angle, front wheel steering angle, longitudinal speed, and longitudinal acceleration respectively; Convert the KKT conditions of the optimization problem into the following matrix form: Among them, is the Hessian matrix of the objective function; is the transpose of the constraint gradient matrix; is the constraint gradient matrix; 0 is the zero matrix; Δq is the search direction of the state variable; Δλ is the update direction of the Lagrange multiplier; is the negative gradient of the Hamiltonian function; -g(q) is the negative value of the constraint function; When calculating the Hessian matrix, the BFGS iterative algorithm is used to continuously update its estimate; after obtaining its optimal solution, the new iterative point q k+1 The update formula is: q k+1 = q k + α k Δq; where α k represents the step size of the current iteration, and its value is determined by the Armijo criterion, and the form of the criterion is: where J(q) is the objective function, g(q) is the constraint function, and ||g(q)||1 is the 1-norm of the constraint function; The Armijo rule requires that α k satisfies the following inequality: Among them, the control parameter ρ ∈ (0, 1) is a constant; are the gradients of the objective function and the constraint function; The stopping criterion of sequential quadratic programming is based on the following conditions: where q k and λ k represent the optimization variable and the Lagrange multiplier at the current iteration step, respectively, H(q k , λ k ) is the Hamiltonian function, g(q k ) is the constraint function, ||·||2 represents the Euclidean norm, and ε is a preset tolerance error threshold.

9. A local trajectory planning method based on a vehicle dynamics model according to claim 8, characterized in that, Step S53 specifically includes: Before performing trajectory optimization, first determine the initial state of the vehicle within a given time interval and the desired target state Under different scenario types, the motion trajectory planning of the vehicle will be adjusted according to the terminal constraint state S of the predetermined target tar In each scenario, according to the target terminal constraint state S tar Sample the entire terminal state. In each time interval t ∈ [T i , T i+1 , perform spatial sampling of the start and end states of S(t). The specific form is as follows: Among them, T i+1 represents the end state time of this trajectory planning, s(T i+1 ) is the displacement of the end state, is the velocity of the end state, is the acceleration of the end state; After the discretized sampling operation, the end state is updated to: Among them, Δs(T i+1 ) is the granularity of displacement sampling. By adjusting the values of Δs(T i+1 ) and T i+1 , the entire trajectory set is further determined.

Images