A method, device, and medium for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on Lagrangian functions.

Abstract

This invention provides a method, device, and medium for optimizing the trajectory of heavy-duty vehicles' intelligent driving based on Lagrange functions, belonging to the field of automatic control and trajectory planning technology. By selecting the vehicle's longitudinal position, lateral position, yaw angle, and control variables, the kinematic model is linearized and discretized at reference trajectory points to construct a state-space equation. Lagrange multipliers are introduced to form the objective function, and the control increment is solved using partial derivatives. Singular value decomposition is used to handle the incomplete rank problem of the matrix. After truncating and correcting the control quantity with actuator constraints, it is substituted into the nonlinear model to update the state variables. Based on environmental perception, vehicle speed and trajectory curvature information are obtained, and the control weights of vehicle speed / steering angle are dynamically adjusted through a preset learning factor to achieve adaptive weighting. Iterative optimization is repeated for all reference trajectory points, finally outputting the optimized trajectory. This invention can generate smoother and more robust trajectories in complex dynamic environments.

Description

Technical Field

[0001] This invention belongs to the field of automatic control and trajectory planning technology, specifically relating to a method, device and medium for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function. Background Technology

[0002] Intelligent driving and Advanced Driver Assistance Systems (ADAS) represent one of the current directions for the integration and development of the automotive industry and artificial intelligence. Driving style recognition methods are mainly based on vehicle speed and acceleration modeling, with models including Support Vector Machines (SVM), Hidden Markov Models (HMM), or Long Short-Term Memory Networks (LSTM).

[0003] In related technologies, it is necessary to assign weights to relevant data, and the allocation method is to balance vehicle speed control and steering angle control. However, in high-speed driving scenarios, the weight allocation of related technologies may lead to excessive priority of steering angle control, causing vehicle speed fluctuations and affecting stability; in scenarios with large curvature curves, excessive weight of vehicle speed control may lead to insufficient steering response and trajectory deviation.

[0004] For heavy-duty vehicle kinematic models, which are typically nonlinear systems, trajectory optimization suffers from high computational complexity and slow convergence, making it difficult to meet the real-time control requirements of intelligent driving. In optimization based on linearized models, when the state error matrix or control error matrix is ​​not of full rank, the solution method is prone to problems such as no solution or excessively fluctuating solutions, affecting the reliability of the intelligent driving system. After updating the model's state, as the driving distance increases, the deviation between the updated state variables and the actual vehicle state gradually widens, causing subsequent optimization calculations to be based on incorrect initial data, ultimately leading to decreased trajectory tracking accuracy and deviation from the reference trajectory. Summary of the Invention

[0005] This invention provides a method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function. The method optimizes the smoothness and robustness of the trajectory by establishing a nonlinear dynamic model, linearizing it at reference trajectory points, and introducing constraints in conjunction with the Lagrangian function.

[0006] The methods include:

[0007] S101: Select vehicle state variables and control variables; linearize the vehicle kinematic model at the reference trajectory point to obtain the linear state-space equation; discretize the linear state-space equation to obtain the relationship expression between the state variables of the next cycle and the current state variables and control variables;

[0008] S102: Define the Lagrange multiplier and construct the Lagrange function with the goal of minimizing the state error and control error; take the partial derivatives of the Lagrange function with respect to the control increment and the Lagrange multiplier and set them to zero to solve the expression for the control increment; when the matrix corresponding to the control increment is not full rank, use singular value decomposition to calculate the matrix pseudo-inverse to obtain the stable minimum norm solution.

[0009] S103: Determine the allowable range of control quantity changes based on the motion constraints of the vehicle actuators; determine whether the control quantity exceeds the allowable range based on the control quantity increment; if it does, truncate and correct the control quantity to meet the allowable range requirements.

[0010] S104: Substitute the control variables obtained in S103 and the state variables of the previous cycle into the nonlinear vehicle kinematics equation to calculate the updated state variables for the next cycle, which are used for trajectory optimization calculation in the next cycle.

[0011] S105: Obtain the current vehicle speed and reference trajectory point curvature information through the environmental perception module; initialize the initial values ​​of the vehicle speed control weight and steering angle control weight;

[0012] S106: Based on the vehicle speed and reference trajectory curvature information obtained in step S105, and combined with the preset vehicle speed / steering angle control weight learning factor, dynamically adjust the values ​​of vehicle speed control weight and steering angle control weight according to the rules.

[0013] S107: Substitute the updated weight coefficients obtained in step S106 into the equation, and repeat steps S101 to S106 for all reference trajectory points to achieve dynamic iterative optimization of the vehicle's driving trajectory.

[0014] S108: After completing the iterative optimization calculation of all reference trajectory points, output the vehicle driving trajectory data after trajectory optimization.

[0015] It should be further explained that step S101 specifically includes the following methods:

[0016] The longitudinal position x, the lateral position y, and the yaw angle ψ are selected as state variables, and the vehicle speed v and the front wheel steering angle δ are selected as control variables.

[0017] Obtain the reference state variables and reference control variables of the reference trajectory; extract the xref, yref, and ψref of each reference point from the pre-planned reference trajectory as reference state variables, and the vref and δref as reference control variables to form the baseline data;

[0018] xref is the longitudinal position reference value of the corresponding reference point on the reference trajectory, yref is the lateral position reference value of the corresponding reference point on the reference trajectory, ψref is the yaw angle reference value of the corresponding reference point on the reference trajectory, vref is the vehicle speed reference value of the corresponding reference point on the reference trajectory, and δref is the front wheel steering angle reference value of the corresponding reference point on the reference trajectory.

[0019] The vehicle kinematics model is linearized at the reference trajectory point, and then the linearized state-space equation is discretized. The output difference between the vehicle kinematics model and the discretized equation near the reference point is compared to ensure that the approximation error is within an acceptable range.

[0020] It should be further explained that step S102 specifically includes the following methods:

[0021] Define state error and control error ,in Formula (4) represents the difference between the current state and the reference state. according to Formula (5) represents the difference between the current control quantity and the reference control quantity, where T is the sampling time;

[0022] Construct a Lagrangian function L with the objective of minimizing the state error and control error, combined with... Formula (7)

[0023] By introducing the Lagrange multiplier λ, discrete kinematic constraints are embedded into the function;

[0024] For the Lagrange function L, respectively with respect to Taking the partial derivatives with λ and setting them to zero, we establish a system of equations and derive the following: The control error expression of formula (8);

[0025] Determine the matrix in formula (8) Is the rank full? If it is full, then proceed as follows: Solve formula (10); if the rank is not full, proceed to step S1025;

[0026] For matrices with less than rank Perform singular value decomposition, according to Formula (11) is used to calculate the pseudo-inverse of the matrix and obtain a stable minimum norm control error solution;

[0027] Where B# denotes the pseudo-inverse of matrix B, which is the generalized inverse matrix when matrix B is not full rank, and is used to find stable minimum norm solutions when the inverse matrix does not exist.

[0028] W is a weighting coefficient matrix that balances vehicle speed v and front wheel steering angle δ;

[0029] BT represents the transpose of matrix B.

[0030] It should be further explained that step S103 specifically includes the following methods:

[0031] Determine the motion constraints of the heavy-duty vehicle actuator, clarify the control quantities of vehicle speed v and front wheel steering angle δ, as well as the maximum change Umax_limit and minimum change Umin_limit under the actuator motion constraints;

[0032] The control quantity increment obtained in step S102 U and reference control quantity according to Formula (12) is used to calculate the initial actual control quantity U(k);

[0033] Initial Compared with the control quantity U(k) of the previous cycle 1) Substitution Formula (13) is used to determine whether the actuator motion constraints are satisfied.

[0034] when season

[0035] Formula (15)

[0036] when season

[0037] Formula (17)

[0038] If the constraints are satisfied hour,

[0039] based on Formula (19) determines the final U(k);

[0040] Verify whether the truncated and corrected U(k) meets the dynamic characteristic requirements of the actuator.

[0041] It should be further explained that step S104 specifically includes the following methods:

[0042] S1041: Extract the vehicle state variables from the trajectory optimization output of the previous cycle, including longitudinal position x(k), lateral position y(k), and yaw angle Ψ(k), and combine them into a state vector X(k) according to the dimensional order defined by the state variables, which serves as the initial input for state update;

[0043] S1042: Read the truncated and corrected control quantity increment output from step S103. U(k), this increment includes components of two dimensions: the change in vehicle speed Δv(k) and the change in front wheel steering angle Δδ(k), which are consistent with the dimensions defined for the control variables;

[0044] S1043: Obtain the control variable U(k) actually executed in the previous cycle. 1), U(k) 1) With step S1042 The control variable U(k) is calculated by summing the corresponding components of U(k) along its dimensions. In other words, each dimension component of U(k) equals the control variable U(k) executed in the current cycle. 1) Add to the corresponding component The corresponding components of U(k);

[0045] S1044: Call the nonlinear kinematic equation of formula (20) to update the state variables.

[0046] Formula (20)

[0047] In the formula, x k , y k and These are the state variables from the previous period. l Wheelbase T Sampling time;

[0048] Using X(k) from step S1041 and U(k) from step S1043 as equation inputs, calculate and output the state vector X(k+1) for the next cycle, which includes three components: longitudinal position x(k+1), lateral position y(k+1), and yaw angle Ψ(k+1).

[0049] S1045: Store the calculated X(k+1) into the system's preset state cache area, mark it as the previous cycle's state variable for the next cycle's trajectory optimization, and complete the update and transmission of the state variable.

[0050] It should be further explained that step S105 specifically includes the following methods:

[0051] Collect current vehicle speed information and curvature information of reference trajectory points;

[0052] Based on the application scenarios of intelligent driving, predefined vehicle speed control weights w v and steering angle control weight w δ The initial value;

[0053] Based on the empirical requirements for controlling priorities in different scenarios, specific values ​​for the learning factors α and β of the weight coefficients are set.

[0054] Establish a scenario-weighted quantization mapping rule library, and explicitly increase the weighting when the curvature of the reference trajectory is large. w δ High-speed scenarios increase w v Quantitative correlation of rules;

[0055] Initial w v and w δ Substitution Formula (9) initializes the weight matrix W, which is used for the optimization calculation of the Lagrange function.

[0056] It should be further explained that step S106 specifically includes the following methods:

[0057] The collected current vehicle speed and reference trajectory curvature data are filtered to remove outliers and noise interference, resulting in smoothed scene feature data.

[0058] Based on the smoothed scene feature data, and by referring to the established scene-weight quantization mapping rule library, the current driving scene type is determined;

[0059] Extract the vehicle speed control weight w from the previous moment. v,k-1 Steering angle control weight w δ，k-1 Based on the set learning factors α and β, and the weight adjustment rule, the following is applied:

[0060] Formula (21)

[0061] Calculate the candidate weight values ​​at the current time.

[0062] Set reasonable ranges for vehicle speed control weights and steering angle control weights, perform boundary constraint verification on the candidate weight values ​​at the current moment, and correct them according to preset thresholds if they exceed the range.

[0063] Substitute the current weight value after boundary constraint verification into formula (9) to update the weight coefficient matrix W, forming an optimized weight constraint that adapts to the current scenario.

[0064] It should be further explained that step S107 specifically includes the following methods:

[0065] Obtain the list of discrete points Tra of the pre-planned reference trajectory, initialize the iteration counter Cnt to 1, and set the maximum number of iterations CntMax and the convergence threshold Thresh for the full trajectory state error.

[0066] The iteration termination condition is defined as Cnt exceeding CntMax or the sum of the full trajectory state errors of two adjacent iterations being less than Thresh, where the full trajectory state error is the sum of the Euclidean distances between the actual state and the reference state of each reference trajectory point.

[0067] Start the iterative loop, where the loop condition satisfies Cnt≤CntMax and error convergence has not been triggered; within the loop body, process each reference trajectory point in order of Tra index;

[0068] For the reference trajectory point being processed, the weight matrix W output in the previous round is used. k 1. The state variable X(k) of the previous period 1) As input, it is passed to the optimization process of steps S101 to S106 to calculate the control quantity increment, the truncated control quantity and the updated state variable at the current point.

[0069] After each iteration of a trajectory point is completed, calculate the current full trajectory state error; determine whether the termination condition is met: if it is met, exit the loop and output the final optimized trajectory; if it is not met, increment Cnt by 1 and return to the start iteration loop step to continue the next iteration.

[0070] According to another embodiment of this application, an electronic device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrange function.

[0071] According to another embodiment of this application, a storage medium is also provided, on which a computer program is stored, which, when executed by a processor, implements the steps of the heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrange function.

[0072] As can be seen from the above technical solutions, the present invention has the following advantages:

[0073] This invention provides a method for optimizing the trajectory of heavy-duty vehicles' intelligent driving based on the Lagrangian function. By linearizing and discretizing the vehicle kinematics model, the complex nonlinear optimization problem is transformed into an easily solvable linear problem, reducing computational resource consumption, improving the real-time performance of optimization calculations, and adapting to the real-time control requirements of heavy-duty vehicle intelligent driving. To address the issue of incomplete matrix rank in the incremental solution of control quantities, singular value decomposition is used to calculate the pseudo-inverse of the matrix, ensuring stable output of optimization results under different driving scenarios. Through actuator motion constraint verification and control quantity truncation correction, the control quantity is prevented from exceeding the maximum acceleration capacity of the power system and the maximum steering angle range of the steering system, ensuring that control commands are accurately responded to by the vehicle. State updates are performed based on the nonlinear vehicle kinematics model, conforming to the actual motion law of the vehicle, ensuring accurate and reliable initial state data for the next optimization cycle. Weights are dynamically adjusted based on real-time collected vehicle speed and reference trajectory curvature information, making the priority of vehicle speed control and steering angle control adapt to the current driving scenario, improving the targeting of multi-objective optimization. By repeatedly executing the optimization process on all reference trajectory points, the disconnect between local trajectory optimization and overall trajectory requirements is avoided, ensuring a smooth transition of the optimized trajectory across the entire road segment. The output trajectory data integrates the optimization information of all reference points, which can be directly adapted to the execution requirements of heavy-duty vehicle control systems and meet the usage requirements. Attached Figure Description

[0074] To more clearly illustrate the technical solution of the present invention, the accompanying drawings used in the description will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0075] Figure 1 The flowchart shows a method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrange function.

[0076] Figure 2 This is a schematic diagram of an electronic device. Detailed Implementation

[0077] This invention provides a method for optimizing the trajectory of heavy-duty vehicles' intelligent driving based on the Lagrangian function. By establishing a nonlinear dynamic model and linearizing it at reference trajectory points, and introducing constraints using the Lagrangian function, the smoothness and robustness of the trajectory are optimized. By dynamically adjusting the control weight matrix of the trajectory, the priority of the trajectory can be adjusted in real time under different environments. This invention can generate smoother and more robust trajectories in complex dynamic environments. This method can be widely applied in fields such as autonomous driving and robot navigation, exhibiting high real-time performance and adaptability.

[0078] The following describes in detail the heavy-duty vehicle intelligent driving trajectory optimization method based on Lagrangian functions that relates to this application. Specific details, such as particular system structures and technologies, are presented for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application can also be implemented in other embodiments without these specific details.

[0079] It should be understood that, when used in this specification, terms include indicating the presence of a described feature, integral, step, operation, element, and / or component, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or collections thereof. The terms include, encompass, have, and variations thereof mean including but not limited to, unless otherwise specifically emphasized.

[0080] The statements such as "one embodiment" or "some embodiments" described in this application mean that one or more embodiments of this application include the specific features, structures, or characteristics described in that embodiment. Therefore, the statements such as "in one embodiment," "in some embodiments," "in other embodiments," and "in still other embodiments" in this application do not necessarily refer to the same embodiment, but rather mean one or more, but not all, embodiments, unless otherwise specifically emphasized.

[0081] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0082] Please see Figure 1 The diagram shows a flowchart of a heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrangian function in a specific embodiment. The method includes:

[0083] S101: Select the vehicle's longitudinal position x, lateral position y, and yaw angle Ψ as state variables, and the vehicle speed v and front wheel steering angle δ as control variables; linearize the vehicle kinematic model at the reference trajectory point to obtain the linear state-space equation; discretize the linear state-space equation to obtain the relationship expression between the state variables of the next cycle and the current state variables and control variables.

[0084] Step S101 specifically includes the following methods:

[0085] S1011: Determine the state variables and control variables of the heavy-duty vehicle kinematic model.

[0086] The longitudinal position x, lateral position y, and yaw angle ψ are selected as state variables, and the vehicle speed v and front wheel steering angle δ are selected as control variables. The significance of each variable in trajectory optimization is clarified.

[0087] In some embodiments, based on the characteristics of the planar motion of heavy-duty vehicles, variables that can fully describe the vehicle's position and attitude are selected as state variables, and variables that directly drive the vehicle's motion are selected as control variables.

[0088] S1012: Obtain the reference state variables and reference control variables of the reference trajectory. Extract the xref, yref, and ψref of each reference point from the pre-planned reference trajectory as reference state variables, and the vref and δref as reference control variables to form the baseline data.

[0089] In some embodiments, xref is the longitudinal position reference value of the corresponding reference point on the reference trajectory, which is the ideal position parameter of the vehicle along the driving direction in the preset coordinate system, and serves as the optimization reference for the longitudinal position x.

[0090] yref is the lateral position reference value of the corresponding reference point on the reference trajectory. It is the ideal position parameter of the vehicle in the preset coordinate system perpendicular to the driving direction, and serves as the optimization reference for the lateral position y.

[0091] ψref is the yaw angle reference value of the corresponding reference point on the reference trajectory. It is the ideal attitude angle that the vehicle should maintain at that reference point. It is the turning angle about the axis perpendicular to the ground and serves as the optimization reference for the yaw angle ψ.

[0092] vref is the vehicle speed reference value at the corresponding reference point on the reference trajectory. It is the ideal driving speed that the vehicle should maintain at that reference point, and serves as the control reference for vehicle speed v.

[0093] δref is the reference value of the front wheel steering angle at the reference point on the reference trajectory. It is the ideal front wheel steering angle that the vehicle should have at that reference point to maintain the reference trajectory, and serves as the control reference for the front wheel steering angle δ.

[0094] S1013: Linearize the vehicle kinematic model at the reference trajectory point. Based on the reference state quantity and reference control quantity, establish the vehicle kinematic model as shown in formula (1), and perform a first-order approximation to obtain the linear state-space equation as shown in formula (2), and establish the linear relationship between the state error and the control error;

[0095] Formula (1)

[0096] Formula (2)

[0097] When the subscript ref is included, it indicates a reference state quantity and a reference control quantity.

[0098] In some embodiments, a first-order approximation of the kinematic model formula (1) is performed at the reference point using Taylor expansion, ignoring higher-order terms, resulting in a linear relationship as shown in formula (2). This embodiment transforms a complex nonlinear problem into a linear problem, facilitating the use of efficient linear optimization methods to solve the trajectory, reducing computational complexity, and improving optimization efficiency.

[0099] S1014: Discretize the linearized state-space equations.

[0100] Using a sampling time T, the continuous linear state-space equation is converted into a discrete form, resulting in the discretized equation shown in formula (6), which describes the relationship between the state error of the next cycle and the current state error and control error.

[0101] Formula (6)

[0102] In some embodiments, the vehicle control system performs control using discrete-time sampling. By combining the sampling time T with a discretization method, the continuous linear equation is transformed into a discrete equation as shown in Equation (6). This adapts the model to the sampling characteristics of the actual control system, ensuring that trajectory optimization can be performed in discrete time steps, and the optimization results can be directly used for vehicle control execution.

[0103] S1015: Compare the output differences between the vehicle kinematics model and the discretized equations near the reference point to ensure that the approximation error is within an acceptable range.

[0104] In some embodiments, the approximation error is evaluated by comparing the outputs of the original nonlinear model and the linearized discretized model. Thus, based on the planar motion characteristics of heavy-duty vehicles, longitudinal position, lateral position, and yaw angle, which can fully describe the vehicle's position and attitude, are selected as state variables, while vehicle speed and front wheel steering angle, which directly drive the vehicle's motion, are selected as control variables. The complex nonlinear vehicle kinematics model is simplified at reference trajectory points, transforming it into easily solvable linear state-space equations. Then, based on the discrete sampling characteristics of the actual vehicle control system, the continuous linear equations are converted into discrete forms, establishing the correlation between state variables and control variables of different periods.

[0105] S102: Define the Lagrange multiplier and construct the Lagrange function with the goal of minimizing the state error and control error; take the partial derivatives of the Lagrange function with respect to the control increment and the Lagrange multiplier and set them to zero to solve the expression for the control increment; when the matrix corresponding to the control increment is not full rank, use singular value decomposition to calculate the matrix pseudo-inverse to obtain the stable minimum norm solution.

[0106] Step S102 specifically includes the following methods:

[0107] S1021: Define state error X(k) and control error U(k), where Formula (4) represents the difference between the current state and the reference state. U(k) press Formula (5) represents the difference between the current control quantity and the reference control quantity, where T is the sampling time.

[0108] In some embodiments, trajectory tracking is transformed into a problem of minimizing state error and control error by defining error variables. , Clearly define the optimization target, making the optimization objective quantifiable and with a clear physical meaning.

[0109] S1022: Construct the Lagrangian function L, aiming to minimize the state error and control error, combined with... Formula (7)

[0110] By introducing the Lagrange multiplier λ, discrete kinematic constraints are embedded into the function.

[0111] In some embodiments, the constrained optimization problem is transformed into an unconstrained problem using the Lagrange multiplier method, and the discrete kinematic constraints are embedded as equality constraints into the Lagrange function.

[0112] This embodiment aims to minimize control error and state error, and transforms the discretized kinematic equations into Lagrange functions by introducing Lagrange multipliers:

[0113] Formula (7)

[0114] In this way, the optimization objectives and constraints are unified, the solution process is simplified, and it is convenient to find the optimal solution directly through differentiation.

[0115] S1023: Regarding the Lagrange function L, respectively with respect to... Find the partial derivatives of U and λ, set the partial derivatives to zero, establish a system of equations, and derive the result. The control error expression of formula (8).

[0116] In some embodiments, taking the partial derivative of the Lagrange function and setting it to zero is a condition for the extremum problem, thereby deriving the analytical relationship between control error and state error. This yields an explicit formula for solving the control error, making optimization feasible and providing a direct basis for calculating the actual control quantity.

[0117] S1024: Determine the matrix in formula (8) Is the rank full? If it is full, then proceed as follows: Solve formula (10); if the rank is not full, proceed to step S1025.

[0118] In some embodiments, the rank of the matrix determines the existence and uniqueness of the solution to the system of equations. When the rank is full, the solution is unique; when the rank is less than full, the solution is not stable enough. Identifying the stability of the solution in advance ensures the robustness of the solution.

[0119] S1025: For matrices that are not of full rank Perform singular value decomposition, according to Formula (11) is used to calculate the pseudo-inverse of the matrix and obtain a stable minimum norm control error solution.

[0120] Here, B# represents the pseudo-inverse of matrix B, which is the generalized inverse matrix when matrix B is not of full rank. It is used to find stable minimum norm solutions when the inverse matrix does not exist.

[0121] W is a weighting coefficient matrix that balances vehicle speed v and front wheel steering angle δ. It is used to coordinate the priority of different control variables in the optimization objective. The larger the value, the higher the priority of the corresponding control variable. It will be dynamically adjusted according to the reference trajectory and vehicle state.

[0122] B T Let B be the transpose of matrix B, where B is the control error matrix. T This is the matrix obtained by interchanging rows and columns of B, used to optimize matrix operations during the solution process.

[0123] In some embodiments, singular value decomposition can decompose any matrix into a combination of singular vectors and singular values. Based on this, the pseudo-inverse of the matrix can be calculated, and the minimum norm solution can be obtained even if the matrix is ​​not full rank, ensuring that the control error solution can be stably obtained in various scenarios.

[0124] It can be seen that by introducing Lagrange multipliers, the optimization objective of minimizing state error and control error is combined with the discretized kinematic constraints to construct a unified Lagrange function. Utilizing the principle of extreme value solving, the optimization problem is transformed into a system of equations by taking the partial derivatives of the Lagrange function with respect to the control increment and the Lagrange multipliers and setting them to zero, thus obtaining the calculation relationship of the control increment. To address the potential problem of matrix incomplete rank leading to solution failure during the control increment solution process, singular value decomposition is used to calculate the matrix pseudo-inverse, ensuring a stable minimum norm solution and improving the robustness and reliability of the optimization solution.

[0125] S103: Determine the allowable range of control quantity changes based on the motion constraints of the vehicle actuator; substitute the control quantity increment obtained in step S102 into the control quantity and determine whether the control quantity exceeds the allowable range; if it does, truncate and correct the control quantity to meet the allowable range requirements.

[0126] Step S103 specifically includes the following methods:

[0127] S1031: Determine the motion constraints of the heavy-duty vehicle actuator, and clarify the control quantities of vehicle speed v and front wheel steering angle δ, as well as the maximum change U under the actuator motion constraints. max_limit and minimum change U min_limit .

[0128] S1032: Increment the control quantity obtained in step S102 U and reference control quantity according to Formula (12) is used to calculate the initial actual control quantity U(k).

[0129] In some embodiments, by solving the matrix The state error matrix is ​​formed by superimposing the reference control quantity and the control quantity increment, as shown in the formula. To solve formula (12), in order to ensure the smoothness and safety of the entire process, the change in U should be kept within a certain range, satisfying the following conditions:

[0130] Formula (13)

[0131] The optimized error is transformed into executable control instructions, enabling seamless execution of the optimized process.

[0132] S1033: Initially Compared with the control quantity U(k) of the previous cycle 1) Substitution Formula (13) is used to determine whether the actuator motion constraints are satisfied.

[0133] In some embodiments, the control quantity is compared with that of the previous cycle, combined with the actuator's maximum change constraint. This identifies whether the control parameters are exceeded. In this way, the risk of exceeding limits can be detected in a timely manner, providing a basis for correction and ensuring smooth control.

[0134] S1034: When season

[0135] Formula (15)

[0136] when season

[0137] Formula (17)

[0138] If the constraints are satisfied hour,

[0139] based on Formula (19) determines the final U(k).

[0140] In some embodiments, for values ​​below the minimum or above the maximum, a truncation strategy corresponding to formulas 15 and 17 is adopted, and if the rule is met, formula (19) is used directly for calculation.

[0141] S1035: Verify whether the truncated and corrected U(k) meets the actuator dynamic characteristic requirements.

[0142] In some embodiments, in addition to amplitude constraints, the actuator's response speed also limits changes in the control quantity to verify its rationality. This further ensures that the control quantity is executable, enabling the actuator to respond smoothly.

[0143] It can be seen that, by combining the physical structure and performance limitations of actuators such as the power system and steering system of heavy-duty vehicles, the maximum allowable range of control quantities such as vehicle speed and front wheel angle is determined; the incremental control quantity obtained from the previous steps is substituted into the actual control quantity calculation, and by comparing it with the allowable range, it is determined whether the control quantity exceeds the load-bearing capacity of the actuator; the control quantity that exceeds the allowable range is truncated and corrected to keep it within the range that the actuator can respond to, to prevent sudden changes in control quantity from causing vehicle trajectory turbulence, and to ensure the executability of control commands.

[0144] S104: Substitute the control variables obtained after processing in step S103 and the state variables of the previous cycle into the nonlinear vehicle kinematics equation to calculate the updated state variables for the next cycle, which are then used for trajectory optimization calculation in the next cycle.

[0145] Step S104 specifically includes the following methods:

[0146] S1041: Extract the vehicle state variables from the trajectory optimization output of the previous cycle, including longitudinal position x(k), lateral position y(k), and yaw angle Ψ(k), and combine them into a state vector X(k) according to the dimensional order defined by the state variables, which serves as the initial input for state update.

[0147] In some embodiments, the vehicle state variables correspond to the longitudinal position, lateral position, and yaw angle defined in S101. When combined into a vector, the order of dimensions must be consistent. Optionally, x(k) corresponds to the first dimension, y(k) to the second dimension, and Ψ(k) to the third dimension.

[0148] It is important to note that the state vector is the input parameter of the kinematic equations, and its dimensions and order must match the model's input specifications; otherwise, it will lead to model calculation errors. Ensuring the accuracy of the underlying data for state updates is crucial to avoid subsequent trajectory calculation deviations caused by incorrect state variables.

[0149] S1042: Read the truncated and corrected control quantity increment output from step S103. U(k) is an increment that includes components of two dimensions: the change in vehicle speed Δv(k) and the change in front wheel steering angle Δδ(k), which are consistent with the dimensions defined for the control variables.

[0150] In some embodiments, The two components of U(k) correspond to the dimensions of the control variables. Optionally, Δv(k) corresponds to the vehicle speed and Δδ(k) corresponds to the front wheel steering angle. The truncated increments meet the actuator constraints and will not be verified further. Here, the control increment is the key to connecting the control inputs of the upper and lower cycles. Using a valid increment can ensure the validity of the control input and ensure that the control change in the input kinematic equation meets the requirements.

[0151] S1043: Obtain the control variable U(k) actually executed in the previous cycle. 1), U(k) 1) With step S1042 The control variable U(k) is calculated by summing the corresponding components of U(k) along its dimensions. In other words, each dimension component of U(k) equals the control variable U(k) executed in the current cycle. 1) Add to the corresponding component The corresponding components of U(k).

[0152] In some embodiments, U(k) 1) This refers to the actual control input executed in the previous cycle. When adding them, they correspond one-to-one according to their dimensions. Here, the control input is cumulative; the current control input equals the input from the previous cycle plus the current adjustment. This accurately reflects the dynamic changes in actual control, ensuring that the input to the kinematic equations truly reflects the vehicle's handling state.

[0153] S1044: Call the nonlinear kinematic equation of formula (20) to update the state variables.

[0154] Formula (20)

[0155] In the formula, x k , y k and These are the state variables from the previous period. l Wheelbase T Sampling time;

[0156] Using X(k) from step S1041 and U(k) from step S1043 as input equations, calculate and output the state vector X(k+1) for the next cycle, which includes three components: longitudinal position x(k+1), lateral position y(k+1), and yaw angle Ψ(k+1).

[0157] In some embodiments, the nonlinear equations need to be substituted with parameters such as wheelbase l and sampling time T; the order of the output X(k+1) dimensions must be consistent with the definition of the state vector. The nonlinear kinematic equations here describe the relationship between the vehicle state and the control input and time, and can predict the state of the next cycle. Utilizing a nonlinear model to capture the nonlinear characteristics of vehicle motion improves the accuracy of state prediction.

[0158] S1045: Store the calculated X(k+1) into the system's preset state cache area, mark it as the previous cycle's state variable for the next cycle's trajectory optimization, and complete the update and transmission of the state variable.

[0159] In some embodiments, the trajectory point index is associated when marking the state of the previous cycle. Trajectory optimization here is an iterative process; the calculation of the next cycle requires the state result of the current cycle. This ensures reliable state transfer, supports closed-loop iterative optimization, and guarantees the consistency and real-time performance of trajectory optimization.

[0160] S105: Obtain the current vehicle speed and reference trajectory point curvature information through the environmental perception module; initialize the initial values ​​of the vehicle speed control weight and steering angle control weight.

[0161] In some embodiments, the actual vehicle speed and curvature information of reference trajectory points are collected in real time using cameras, radar, high-precision maps, etc. This information serves as the core basis for judging the characteristics of the driving scenario. Based on the basic control requirements of different driving scenarios for heavy-duty vehicles, initial values ​​for speed control weights and steering angle control weights are preset.

[0162] This allows for the acquisition of key data for scene perception, enabling weight adjustments to align with the actual driving environment. The reasonable setting of initial weights avoids strategy oscillations during dynamic adjustments, ensuring the stability and convergence of weight adjustments.

[0163] S106: Based on the vehicle speed and reference trajectory curvature information obtained in step S105, and combined with the preset vehicle speed / steering angle control weight learning factor, adjust the values ​​of vehicle speed control weight and steering angle control weight according to the rules.

[0164] In some embodiments, based on the vehicle speed and reference trajectory curvature information obtained from environmental perception, and combined with preset vehicle speed control weight learning factors and steering angle control weight learning factors, the weight values ​​of the previous moment are dynamically adjusted according to the rule that steering control is emphasized when the curvature is large and vehicle speed control is emphasized when the speed is high, so as to achieve adaptive optimization of vehicle speed control weight and steering angle control weight.

[0165] S107: Substitute the updated weight coefficients obtained in step S106 into the equation, and repeat steps S101 to S106 for all reference trajectory points to achieve dynamic iterative optimization of the vehicle's driving trajectory.

[0166] In some embodiments, the dynamically adjusted updated weight coefficients are fed back to the initial stage of trajectory optimization. Following the order of the reference trajectory points, the process of model linearization and discretization, optimization of control increment, control constraint correction, state variable update, and weight adjustment is repeatedly executed for each reference trajectory point. Through multiple iterations, dynamic optimization of the entire reference trajectory is achieved. This ensures that the optimization of each reference trajectory point is based on the latest scene information and weight strategy, guaranteeing the consistency and coherence of the entire trajectory optimization.

[0167] S108: After completing the iterative optimization calculation of all reference trajectory points, output the vehicle driving trajectory data after trajectory optimization.

[0168] In some embodiments, after all reference trajectory points have completed iterative optimization calculations, the optimized state variables and control variables of each point are integrated to form a vehicle driving trajectory dataset. This trajectory data is then output according to the receiving requirements of the vehicle control system, providing a direct basis for the actual driving control of the vehicle and ensuring that the trajectory optimization results can effectively guide vehicle driving.

[0169] In one embodiment of the present invention, based on step S105, the following will provide a possible embodiment and describe its specific implementation in a non-limiting manner. Step S105 specifically includes the following methods:

[0170] S1051: Collect current vehicle speed information and curvature information of reference trajectory points.

[0171] In some embodiments, the driving scenario type can be determined by acquiring the vehicle's own motion state and the curvature of the reference trajectory. This provides realistic, real-time scenario data support for adaptive weight adjustment, ensuring that the weight adjustment conforms to the actual driving environment and improving the adaptability of trajectory optimization in complex scenarios.

[0172] S1052: Predefine vehicle speed control weights w based on the application scenarios of intelligent driving (such as high-speed driving and mountain curve driving). v and steering angle control weight w δ The initial value.

[0173] In some embodiments, the initial weights provide a baseline starting point for the adjustment process. The preset initial weights for different scenarios reflect the basic priority tendency of the control variables in that scenario. This ensures that the adaptive adjustment of weights starts from a reasonable range, guaranteeing the stability and convergence of the weight adjustment.

[0174] Step S1053: Based on the experience requirements of control priority in different scenarios, set the specific values ​​of the learning factors α and β of the weight coefficients.

[0175] In some embodiments, learning factors α and β determine the rate at which weights change with scene information, and their values ​​balance the timeliness of weight adjustment response with process stability. Through empirically set learning factors, the weights can quickly adapt to scene changes, ensuring the effectiveness of multi-objective priority coordination.

[0176] Step S1054: Establish a scene-weight quantization mapping rule library, and specify that when the curvature of the reference trajectory is large, increase the weight quantization mapping rule library. w δ High-speed scenarios increase w v Quantitative correlation of rules.

[0177] In some embodiments, qualitative methods such as scenario type and weight adjustment direction / magnitude are transformed into a quantifiable rule base to clarify w under different scenarios. v and w δ The basis for adjustment. It can eliminate the subjective uncertainty of weight adjustment.

[0178] Step S1055: Set the initial w v and w δ Substitution Formula (9) initializes the weight matrix W, which is used for the optimization calculation of the Lagrange function.

[0179] In some embodiments, the weight matrix formula (9) plays a role in balancing the priority of control variables in the Lagrangian function, and the initialized matrix injects scenario-based weight constraints into the optimization process. The scenario-aware weight strategy can be transformed into mathematical optimization constraints, so that trajectory optimization has scenario specificity from the initial stage, improving the smoothness and safety of the optimization results.

[0180] In one embodiment of the present invention, based on step S106, the following will provide a possible embodiment and describe its specific implementation in a non-limiting manner. Step S106 specifically includes the following methods:

[0181] Step S1061: Filter the collected current vehicle speed and reference trajectory curvature data to remove outliers and noise interference, and obtain smoothed scene feature data.

[0182] S1062: Based on the smoothed scene feature data, and by referring to the established scene-weight quantization mapping rule library, determine the current driving scene type.

[0183] In some embodiments, the quantified scene feature data is matched with a preset rule base to achieve accurate classification of driving scenarios, so that the weight adjustment can accurately match the needs of the current driving scenario.

[0184] S1063: Extract the vehicle speed control weight w from the previous moment. v,k-1 Steering angle control weight w δ，k-1 Based on the learning factors α and β set in step S1053, the weight adjustment rule is as follows:

[0185] Formula (21)

[0186] The candidate weight values ​​at the current time are calculated.

[0187] In some embodiments, formula (21) adopts an iterative update logic, based on the weight of the previous moment, and dynamically corrects it by combining the learning factor and scene data to achieve continuous and smooth adjustment of the weight. This ensures that the weight change is continuous and gradual, guaranteeing the stability of the trajectory optimization process.

[0188] S1064: Set a reasonable range for the vehicle speed control weight and steering angle control weight, perform boundary constraint verification on the candidate weight values ​​at the current moment, and correct them according to the preset threshold if they exceed the range.

[0189] In some embodiments, an excessively large weight may lead to an overly high priority for the corresponding control variable, potentially ignoring other control objectives; conversely, an excessively small weight may fail to achieve the optimization effect of the control variable. Boundary constraints can limit the weight to within an effective range. S1064 ensures that the priorities of the two control variables are always in a reasonably coordinated state.

[0190] S1065: Substitute the current weight value after boundary constraint verification into formula (9) to update the weight coefficient matrix W, forming an optimized weight constraint that adapts to the current scenario.

[0191] In some embodiments, the weight coefficient matrix W is the core constraint parameter of the Lagrangian function. The updated matrix can transform the weight strategy of the current scenario into mathematical optimization constraints. This embodiment promptly transmits the scenario-adapted weights to the subsequent trajectory optimization process, ensuring that the solution of the Lagrangian function always fits the requirements of the current scenario, thus guaranteeing the scenario adaptability and safety of the optimization results.

[0192] In one embodiment of the present invention, based on step S107, the following will provide a possible embodiment and its specific implementation will be described in a non-limiting manner. Step S107 specifically includes the following methods:

[0193] S1071: Obtain the list of discrete points Tra of the pre-planned reference trajectory, initialize the iteration counter Cnt to 1, and set the maximum number of iterations CntMax and the convergence threshold Thresh for the full trajectory state error.

[0194] It should be noted that the list of discrete points Tra of the reference trajectory can contain a trajectory point index, a reference state variable, and a reference control variable for each element. The maximum number of iterations CntMax can be set to 10 to 20 times, and the convergence threshold Thresh for the full trajectory state error can be 0.1m to 0.18m.

[0195] In some embodiments, the reference trajectory point list Tra is consistent with the path planning output, and the discrete spacing matches the sampling time T of step S101; the iteration counter Cnt is used to control the number of iterations, and the maximum number of iterations CntMax prevents the algorithm from getting stuck in an infinite loop due to anomalies such as sensor noise. Here, the input boundaries and range of the iteration are clearly defined, providing clear starting conditions for the iteration and ensuring that the process can be executed repeatedly.

[0196] S1072: Define the iteration termination condition as Cnt exceeding CntMax or the sum of the full trajectory state errors of two adjacent iterations being less than Thresh, where the full trajectory state error is the sum of the Euclidean distances between the actual state and the reference state of each reference trajectory point.

[0197] In some embodiments, the total trajectory state error is accumulated by summing the Euclidean distance between the longitudinal position, lateral position, and yaw angle errors of each point. The error convergence threshold Thresh is calibrated based on the accuracy requirements of the heavy-duty vehicle trajectory. This quantifies the degree of iterative convergence, balances optimization accuracy and real-time performance, and ensures that the algorithm terminates in a timely manner.

[0198] S1073: Start the iterative loop, where the loop condition satisfies Cnt≤CntMax and error convergence is not triggered; within the loop body, process each reference trajectory point in the order of Tra index.

[0199] In some embodiments, the iterative loop processes points sequentially, associating them with trajectory point indices. The sequential processing within the loop body ensures that the optimization of each point depends on the results of preceding points. This achieves point-by-point optimization of the entire trajectory, improving the consistency of the overall trajectory.

[0200] S1074: For the reference trajectory point being processed, the weight matrix W output in the previous round will be... k 1. The state variable X(k) of the previous period 1) As input, it is passed to the optimization process of steps S101 to S106 to calculate the control quantity increment, the truncated control quantity and the updated state variable at the current point.

[0201] In some embodiments, the weight matrix W is transmitted. k 1. Dynamic adjustment results from S106, state variable X(k) 1) Output from S104. During transmission, the dimensions and format must be consistent to prevent data misalignment. This involves the closed-loop transmission of states and weights during iteration, enabling feedback through optimization, updates, and further optimizations. This ensures that the optimization of each point adapts to the current weights and states, improving trajectory coordination.

[0202] S1075: After completing the iterative processing of each trajectory point, calculate the current full trajectory state error; determine whether the termination condition is met: if it is met, exit the loop and output the final optimized trajectory; if it is not met, increment Cnt by 1 and return to step S1073 to continue the next round of iteration.

[0203] In some embodiments, the total trajectory state error is the sum of the errors at each point, rather than the error at a single point; the termination condition must simultaneously satisfy the number of iterations and error convergence, comprehensively evaluate the optimization progress and results, and ensure that the trajectory meets the accuracy requirements.

[0204] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0205] like Figure 2 As shown, this application also provides an electronic device, including a display module 103, a memory 102, a processor 101, a communication module 104, and a computer program stored in the memory and executable on the processor 101. When the processor 101 executes the program, it implements the steps of a heavy-duty vehicle intelligent driving trajectory optimization method based on a Lagrange function.

[0206] In embodiments of the present invention, electronic devices include, but are not limited to, laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. Electronic devices may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the embodiments described and / or claimed herein.

[0207] In this embodiment, processor 101 may be implemented using at least one of an application-specific integrated circuit, a programmable logic device, a field-programmable gate array, a processor, a controller, a microcontroller, a microprocessor, or an electronic unit designed to perform the functions described herein. In some cases, such an implementation may be implemented within a controller. For software implementation, implementations such as processes or functions may be implemented with separate software modules that allow the performance of at least one function or operation. Software code may be implemented by a software application (or program) written in any suitable programming language, and the software code may be stored in memory and executed by the controller.

[0208] The display module 103 is used to display information input by the user or information provided to the user. The display module 103 may include a display panel, which may be configured in the form of a liquid crystal display, an organic light-emitting diode, or the like.

[0209] The memory 102 can be used to store software programs and various data. The memory 102 may include high-speed random access memory, and may also include non-volatile memory, such as at least one disk storage device, flash memory device, or other volatile solid-state storage device.

[0210] The communication module 104 transmits radio signals to and / or receives radio signals from at least one of a base station, an external terminal, and a server. Such radio signals may include voice call signals, video call signals, or various types of data sent and / or received according to text and / or multimedia messages.

[0211] The present invention also provides a storage medium storing a computer program thereon, wherein the computer program, when executed by a processor, implements the steps of the heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrange function.

[0212] The storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example,, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples (a non-exhaustive list) of readable storage media include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0213] The storage medium stores a program product capable of implementing the methods described above in this specification. In some possible implementations, various aspects of this disclosure can also be implemented as a program product comprising program code that, when run on a terminal device, causes the terminal device to perform the steps described in the exemplary methods section of this specification according to various exemplary embodiments of this disclosure.

[0214] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function, characterized in that the method... include: S101: Select vehicle state variables and control variables; The vehicle kinematics model is linearized at the reference trajectory points to obtain the linear state-space equations. Discretize the linear state-space equation to obtain the relationship expression between the state variables of the next cycle and the current state variables and control variables; S102: Define the Lagrange multiplier and construct the Lagrange function with the goal of minimizing the state error and control error; take the partial derivatives of the Lagrange function with respect to the control increment and the Lagrange multiplier and set them to zero to solve the expression for the control increment; when the matrix corresponding to the control increment is not full rank, use singular value decomposition to calculate the matrix pseudo-inverse to obtain the stable minimum norm solution. S103: Determine the allowable range of control quantity changes based on the motion constraints of the vehicle actuators; determine whether the control quantity exceeds the allowable range based on the control quantity increment; If the value exceeds the limit, the control quantity is truncated and corrected to meet the allowable range requirements. S104: Substitute the control variables obtained in S103 and the state variables of the previous cycle into the nonlinear vehicle kinematics equation to calculate the updated state variables for the next cycle, which are used for trajectory optimization calculation in the next cycle. S105: Obtain the current vehicle speed and reference trajectory point curvature information through the environmental perception module; initialize the initial values ​​of the vehicle speed control weight and steering angle control weight; S106: Based on the vehicle speed and reference trajectory curvature information obtained in step S105, and combined with the preset vehicle speed / steering angle control weight learning factor, dynamically adjust the values ​​of vehicle speed control weight and steering angle control weight according to the rules. S107: Substitute the updated weight coefficients obtained in step S106 into the equation, and repeat steps S101 to S106 for all reference trajectory points to achieve dynamic iterative optimization of the vehicle's driving trajectory. S108: After completing the iterative optimization calculation of all reference trajectory points, output the vehicle driving trajectory data after trajectory optimization.

2. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S101 specifically includes the following methods: The longitudinal position x, the lateral position y, and the yaw angle ψ are selected as state variables, and the vehicle speed v and the front wheel steering angle δ are selected as control variables. Obtain the reference state variables and reference control variables of the reference trajectory; extract the xref, yref, and ψref of each reference point from the pre-planned reference trajectory as reference state variables, and the vref and δref as reference control variables to form the baseline data; xref is the longitudinal position reference value of the corresponding reference point on the reference trajectory, yref is the lateral position reference value of the corresponding reference point on the reference trajectory, ψref is the yaw angle reference value of the corresponding reference point on the reference trajectory, vref is the vehicle speed reference value of the corresponding reference point on the reference trajectory, and δref is the front wheel steering angle reference value of the corresponding reference point on the reference trajectory. The vehicle kinematics model is linearized at the reference trajectory point, and then the linearized state-space equation is discretized. The output difference between the vehicle kinematics model and the discretized equation near the reference point is compared to ensure that the approximation error is within an acceptable range.

3. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S102 specifically includes the following methods: Define state error X(k) and control error U(k), where This represents the difference between the current state and the reference state. U(k) press , representing the difference between the current control quantity and the reference control quantity, where T is the sampling time; Construct a Lagrangian function L with the objective of minimizing the state error and control error, combined with Formula (7) By introducing the Lagrange multiplier λ, discrete kinematic constraints are embedded into the function; For the Lagrange function L, respectively with respect to Find the partial derivatives of U and λ, set the partial derivatives to zero, establish a system of equations, and derive the result. The control error expression; Determine the matrix in formula (8) Is the rank full? If it is full, then proceed as follows: Solve; if the rank is not full, proceed to step S1025; For matrices with less than rank Perform singular value decomposition, according to Calculate the matrix pseudoinverse to obtain a stable minimum norm control error solution; Where B# denotes the pseudo-inverse of matrix B, which is the generalized inverse matrix when matrix B is not full rank, and is used to find stable minimum norm solutions when the inverse matrix does not exist. W is a weighting coefficient matrix that balances vehicle speed v and front wheel steering angle δ; B T Let B be the transpose of matrix B.

4. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrange function according to claim 1, characterized in that, Step S103 specifically includes the following methods: Determine the motion constraints of the heavy-duty vehicle actuator, and clarify the control quantities of vehicle speed v and front wheel steering angle δ, as well as the maximum change U under the actuator motion constraints. max_limit and minimum change U min_limit ; The control quantity increment obtained in step S102 U and reference control quantity according to Calculate the initial actual control quantity U(k); Initial Compared with the control quantity U(k) of the previous cycle 1) Substitution Determine whether the actuator motion constraints are satisfied; when season Official (15) when season Official (17) If the constraints are satisfied hour, based on Determine the final U(k); Verify whether the truncated and corrected U(k) meets the dynamic characteristic requirements of the actuator.

5. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S104 specifically includes the following methods: S1041: Extract the vehicle state variables from the trajectory optimization output of the previous cycle, including longitudinal position x(k), lateral position y(k), and yaw angle Ψ(k), and combine them into a state vector X(k) according to the dimensional order defined by the state variables, which serves as the initial input for state update; S1042: Read the truncated and corrected control quantity increment output from step S103. U(k) is an increment that includes components of two dimensions: the change in vehicle speed Δv(k) and the change in front wheel steering angle Δδ(k), which are consistent with the dimensions defined for the control variables. S1043: Obtain the control variable U(k) actually executed in the previous cycle. 1), U(k) 1) With step S1042 The control variable U(k) is calculated by summing the corresponding components of U(k) along its dimensions. In other words, each dimension component of U(k) equals the control variable U(k) executed in the current cycle. 1) Add to the corresponding component The corresponding components of U(k); S1044: Call the nonlinear kinematic equation of formula (20) to update the state variables. Official (20) In the formula, x k , y k and These are the state variables from the previous period. l Wheelbase T Sampling time; Using X(k) from step S1041 and U(k) from step S1043 as equation inputs, calculate and output the state vector X(k+1) for the next cycle, which includes three components: longitudinal position x(k+1), lateral position y(k+1), and yaw angle Ψ(k+1). S1045: Store the calculated X(k+1) into the system's preset state cache area, mark it as the previous cycle's state variable for the next cycle's trajectory optimization, and complete the update and transmission of the state variable.

6. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S105 specifically includes the following methods: Collect current vehicle speed information and curvature information of reference trajectory points; Based on the application scenarios of intelligent driving, predefined vehicle speed control weights w v and steering angle control weight w δ The initial value; Based on the empirical requirements for controlling priorities in different scenarios, specific values ​​for the learning factors α and β of the weight coefficients are set. Establish a scenario-weighted quantization mapping rule library, and explicitly increase the weighting when the curvature of the reference trajectory is large. w δ High-speed scenarios increase w v Quantitative correlation of rules; Initial w v and w δ Substitution Initialize the weight matrix W for the optimization calculation of the Lagrange function.

7. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S106 specifically includes the following methods: The collected current vehicle speed and reference trajectory curvature data are filtered to remove outliers and noise interference, resulting in smoothed scene feature data. Based on the smoothed scene feature data, and by referring to the established scene-weight quantization mapping rule library, the current driving scene type is determined; Extract the vehicle speed control weight w from the previous moment. v,k-1 Steering angle control weight w δ，k-1 Based on the set learning factors α and β, and the weight adjustment rule, the following is applied: Official (21) Calculate the candidate weight values ​​at the current time. Set reasonable ranges for vehicle speed control weights and steering angle control weights, perform boundary constraint verification on the candidate weight values ​​at the current moment, and correct them according to preset thresholds if they exceed the range. Substitute the current weight value after boundary constraint verification into formula (9) to update the weight coefficient matrix W, forming an optimized weight constraint that adapts to the current scenario.

8. The method for optimizing the trajectory of intelligent driving of heavy-duty vehicles based on the Lagrangian function according to claim 1, characterized in that, Step S107 specifically includes the following methods: Obtain the list of discrete points Tra of the pre-planned reference trajectory, initialize the iteration counter Cnt to 1, and set the maximum number of iterations CntMax and the convergence threshold Thresh for the full trajectory state error. The iteration termination condition is defined as Cnt exceeding CntMax or the sum of the full trajectory state errors of two adjacent iterations being less than Thresh, where the full trajectory state error is the sum of the Euclidean distances between the actual state and the reference state of each reference trajectory point. Start the iterative loop, where the loop condition satisfies Cnt≤CntMax and error convergence has not been triggered; within the loop body, process each reference trajectory point in order of Tra index; For the reference trajectory point being processed, the weight matrix W output in the previous round is used. k 1. The state variable X(k) of the previous period 1) As input, it is passed to the optimization process of steps S101 to S106 to calculate the control quantity increment, the truncated control quantity and the updated state variable at the current point. After each iteration of a trajectory point is completed, calculate the current full trajectory state error; determine whether the termination condition is met: if it is met, exit the loop and output the final optimized trajectory; if it is not met, increment Cnt by 1 and return to the start iteration loop step to continue the next iteration.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrange function as described in any one of claims 1 to 8.

10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the heavy-duty vehicle intelligent driving trajectory optimization method based on the Lagrange function as described in any one of claims 1 to 8.

Images