Intelligent vehicle trajectory tracking control method based on dung beetle optimization algorithm

By introducing a dung optimization algorithm to optimize LQR and MPC controllers in intelligent vehicle trajectory tracking control, the problem of insufficient longitudinal speed control in the existing technology is solved, and high-precision trajectory tracking and stability under different vehicle speeds and road conditions are achieved , suitable for a variety of working conditions.

CN118963351AInactive Publication Date: 2024-11-15ANHUI UNIVERSITY OF TECHNOLOGY

Patent Information

Application Number
CN202411040180.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-31
Publication Date
2024-11-15
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing intelligent vehicle trajectory tracking control methods are difficult to ensure the precise driving and stability of the vehicle along the planned trajectory at different vehicle speeds, especially the longitudinal speed control is not fully considered, resulting in insufficient tracking accuracy and stability.

Method used

Using the intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm, the trajectory tracking error model is constructed, and the horizontal LQR controller is designed and the vertical MPC controller is designed, combining the vehicle's speed and steering is used to optimize LQR using the dung beetle optimization algorithm. Weight matrix parameters and simulation experiments were performed by MATLAB and Carsim.

Benefits of technology

It realizes high-precision trajectory tracking under different vehicle speeds and road conditions, reduces lateral errors and heading deviations, improves the stability and tracking accuracy of the vehicle, and is suitable for various working conditions such as urban roads and highways.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118963351A_ABST
    Figure CN118963351A_ABST
Patent Text Reader

Abstract

The invention discloses an intelligent vehicle trajectory tracking control method based on a dung beetle optimization algorithm, and belongs to the technical field of pilotless vehicles. The method comprises the following steps: 1, acquiring real-time state information of a controlled vehicle, establishing a safety constraint condition according to the real-time state information, establishing a lane changing trajectory function of the controlled vehicle based on a quintic polynomial, and acquiring optimal trajectory information according to the lane changing trajectory function; 2, constructing a trajectory tracking error model; 3, designing a transverse linear quadratic regulation controller based on a dung beetle optimization algorithm, and 4, designing longitudinal speed tracking based on model predictive control, connecting the transverse controller and the longitudinal controller by utilizing longitudinal speed, and controlling the speed and steering of the vehicle at the same time, so as to realize trajectory tracking control of the controlled vehicle. According to the invention, the vehicle dynamics model is simplified, and the trajectory tracking efficiency is improved; and the model is linearized and discretized, so that the real-time performance of the algorithm is enhanced, and the control precision, stability and reliability are improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of driverless vehicles, and more specifically, relates to a smart vehicle trajectory tracking control method based on a dung beetle optimization algorithm. Background Art

[0002] In recent years, due to the increasing prominence of problems such as traffic safety and pollution, smart cars have become an important part of smart transportation and have attracted much attention. At present, many domestic and foreign automobile companies have invested huge amounts of money in the field of autonomous driving and introduced a large number of talents to carry out technology research and development and innovation, aiming to enhance their competitiveness in this field. As one of the core technologies of intelligent driving, the lateral and longitudinal control of smart cars processes the planned trajectory and vehicle position information, outputs corresponding control commands, and ensures that the vehicle travels along the predetermined trajectory. This mainly involves lateral trajectory tracking and longitudinal speed tracking. Lateral control mainly controls the steering system of the vehicle according to the control algorithm to allow the vehicle to accurately track the desired trajectory. Longitudinal control mainly controls the throttle and brake to achieve the purpose of tracking the desired vehicle speed. In essence, it controls the speed and position of the vehicle. How to ensure that the autonomous driving vehicle travels accurately along the planned trajectory and achieves the expected tracking accuracy is a major problem in this field.

[0003] At present, path tracking control methods mainly include proportional differential integral control, sliding mode control, pure tracking control, model predictive control and adaptive control. The evaluation index of LQR control is the quadratic integral of the state variable of the controlled linear system. The optimal control rate is obtained by solving the Riccati equation to achieve the closed-loop control target. This method has low requirements on hardware computing power, and the car has excellent tracking effect at different speeds. MPC control can take into account the system state and control input at future time points, which improves the predictability and stability of control.

[0004] After searching, the patent application document with Chinese patent application number 202210036507.7 and application publication date of April 22, 2022 discloses an improved path tracking control method based on LQR. This method is based on the lateral deviation value and heading deviation value between the current actual position of the unmanned vehicle and the planned position. The fuzzy controller is used to perform real-time correction and optimization on the elements of Q in the original weighting matrix of the LQR controller to obtain the optimized weighting matrix Q. The control amount of the front wheel steering angle of the vehicle is obtained by solving the LQR problem. However, only the lateral control of the vehicle is considered, and the control of the longitudinal speed is not considered. The reference path design is too simple, and the experimental results are not compared with the LQR controller that is not optimized using the fuzzy controller. Summary of the invention

[0005] 1. Problems to be solved by the invention

[0006] In order to ensure the accuracy and stability of intelligent vehicle trajectory tracking, a trajectory tracking control method for intelligent vehicles based on the dung beetle optimization algorithm is proposed. First, the trajectory tracking error model is constructed. Then, the lateral linear quadratic regulation (LQR) controller based on the dung beetle optimization algorithm is designed. The longitudinal speed is used to connect the lateral and longitudinal controllers, and the speed and steering of the vehicle are controlled at the same time. Finally, simulation experiments are carried out based on MATLAB and Carsim.

[0007] 2. Technical solution

[0008] In order to achieve the above object, the technical solution provided by the present invention is:

[0009] The present invention provides a smart vehicle trajectory tracking control method based on a dung beetle optimization algorithm, the steps of which are as follows:

[0010] Step 1: obtain the real-time status information of the controlled vehicle, establish safety constraints based on the real-time status information, establish a lane-changing trajectory function of the controlled vehicle based on multiple polynomials, and obtain the optimal lane-changing trajectory related parameters according to the lane-changing trajectory function;

[0011] Step 2: Establish a dynamic model of the controlled vehicle, and establish a trajectory tracking error model based on the lane change trajectory function and the dynamic model;

[0012] Step 3: Introduce the improved dung beetle optimization algorithm (DBO) to optimize the linear quadratic regulation (LQR) weight matrix parameters, optimize the trajectory tracking error according to the feedforward + feedback LQR control algorithm to obtain the front wheel angle of the controlled vehicle, and make lateral adjustments to the controlled vehicle;

[0013] Step 4: Design a longitudinal model predictive control (MPC) to achieve speed tracking. Use the longitudinal speed to connect the lateral and longitudinal controllers, and control the speed and steering of the vehicle at the same time to achieve trajectory tracking control of the controlled vehicle.

[0014] Step 5: Perform joint simulation based on Simulink and Carsim.

[0015] Furthermore, in step 1, the real-time state information of the controlled vehicle is obtained, and safety constraints are established based on the real-time state information. The lane-changing trajectory function of the controlled vehicle is established based on a quintic polynomial, and the optimal lane-changing trajectory related parameters are obtained according to the lane-changing trajectory function:

[0016] The reference trajectory is set using a quintic polynomial, which has a smooth and continuous curvature, making the tracking effect more ideal, and can be expressed as

[0017]

[0018] x(t) and y(t) represent the longitudinal and lateral displacements of the vehicle, and a0, a1, a2, a3, a4, a5, b0, b1, b2, b3, b4, b5 are unknown coefficients.

[0019] In addition, according to the Urban Road Engineering Design Code CJJ 37-2012 (2016 Edition), the reference trajectory needs to meet the constraints of the start and end points:

[0020]

[0021] Where x represents the longitudinal displacement, represents the longitudinal velocity, represents longitudinal acceleration, y represents lateral displacement, y′ represents lateral velocity, y″ represents lateral acceleration, T represents the time taken to travel from the starting point to the end point, and x end is the abscissa of the end point. By setting different constraints, we can solve the corresponding unknown coefficients a0, a1, a2, a3, a4, a5, b0, b1, b2, b3, b4, b5. At the same time, convert y(x) into y(t) to facilitate the solution of the ordinate:

[0022]

[0023] Then solve for the tangent angle θ of the desired trajectory p and curvature k p

[0024]

[0025] Where: θ p is the tangent angle of the desired trajectory, k p is the curvature of the desired trajectory, y p ′ is the desired lateral velocity, y p ″ is the expected lateral acceleration;

[0026] Finally, the reference trajectory also limits the range of acceleration:

[0027]

[0028] Furthermore, the process of establishing the dynamic model of the controlled vehicle in step 2 and establishing the trajectory tracking error model according to the lane change trajectory function and the dynamic model is as follows:

[0029] The dynamic model of the controlled vehicle is:

[0030]

[0031] in, is the lateral velocity, is the yaw angular velocity, is the lateral acceleration, is the yaw angular acceleration, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, I z is the moment of inertia of the vehicle, δ is the front wheel turning angle;

[0032] set up u=δ, transform the above formula into a general form

[0033]

[0034] Among them: η is the state variable matrix; A is the state variable coefficient matrix; B is the control variable coefficient matrix; u is the control variable. Therefore, y and control.

[0035] The trajectory tracking error model is:

[0036]

[0037] Among them, e d is the lateral displacement error, is the lateral velocity error, is the lateral acceleration error, is the yaw angle error, is the yaw rate error, is the yaw angular acceleration error, is the heading angular velocity, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, I z is the moment of inertia of the vehicle, and δ is the front tire slip angle.

[0038] Furthermore, the Dung Beetle Optimization Algorithm (DBO) described in step 3 is introduced to optimize the LQR weight matrix parameters, and the trajectory tracking error is optimized according to the feedforward + feedback LQR control algorithm to obtain the front wheel steering angle of the controlled vehicle. When the optimal front wheel steering angle of the LQR controller is calculated based on the trajectory tracking error model, simplifying the model will bring unavoidable steady-state errors to the controlled system. In order to eliminate the steady-state error, it is necessary to introduce an additional feedforward steering angle control amount δ f Make rr Converge to 0 as much as possible, then redefine the full state feedback control and error differential equation:

[0039]

[0040] When control is stable:

[0041]

[0042] When the lateral error e d =0, solve the above equation to get the wheel angle feedforward:

[0043]

[0044] Where k3 is the third column of the feedback matrix K.

[0045] The two weight coefficients Q and R in LQR control represent the limiting effects on the system state quantity and control quantity respectively, which affect the vehicle trajectory tracking effect and can be written in the following matrix form:

[0046] Q=diag[q1,q2,q3,q4],R=[r]

[0047] In general, the larger the value of the elements in the Q matrix, the faster the system responds. The larger the value of the elements in the R matrix, the greater the limit on the control quantity. Since the weight coefficient determination process of the LQR controller is complex and usually determined by experience, the dung beetle optimization algorithm is used to determine the three parameters q1, q3 and r of the diagonal of the weighted matrices Q and R. In order to ensure both driving stability and trajectory tracking capability, the lateral distance deviation e is used. d , heading angle deviation and the front wheel steering angle δ f The weighted sum of squares of the changes is used as the fitness function of the dung beetle algorithm optimization:

[0048]

[0049] Since the emphasis is placed on the effect of trajectory tracking, the weighting coefficients ψ1=0.4, ψ2=0.35, and ψ3=0.25 are set. The maximum number of iterations is set to 30, and the initial number of individuals in the dung beetle population is 30.

[0050] Furthermore, the design of longitudinal model predictive control (MPC) described in step 4 realizes speed tracking, and uses the longitudinal speed to connect the lateral and longitudinal controllers, and simultaneously controls the speed and steering of the vehicle to realize the trajectory tracking control process of the controlled vehicle as follows:

[0051] Assuming that the vehicle's yaw motion is small, we have:

[0052]

[0053] In the formula, a xis the longitudinal acceleration, is the longitudinal acceleration;

[0054] The lower controller is established by approximating the first-order inertial system, as shown in Eq.

[0055]

[0056] Where τ is the inertia time constant; a is the longitudinal acceleration; a p is the expected longitudinal acceleration. The vehicle longitudinal motion equation can be described as follows, where v is the vehicle longitudinal velocity.

[0057]

[0058] Take the state quantity ξ=[v,a,] T , control quantity u=a p , output O(t), the above formula is described as the state space expression

[0059]

[0060] t, t+1, and T are the current sampling time point, the next cycle sampling time point, and the sampling cycle, respectively.

[0061] Write the prediction equation in matrix form, as follows:

[0062] Y(t)=φξ(t)+ΘU(t)

[0063] In the formula,

[0064] N p ,N c They are respectively the prediction time domain and the control time domain. After establishing the model for predicting the state of the controlled vehicle, the next step of rolling optimization is carried out. The state optimization problem at time point t can be specifically expressed as: determine the m control quantities u(t), u(t+1), u(t+2), ..., u(t+m-1) from this time point, so that the state of the controlled vehicle under its influence at the next j time points is close to the expected state to the maximum extent, preventing the vehicle speed from changing sharply. Therefore, the optimization performance index can be expressed in the following vector form:

[0065]

[0066] Among them, S and W are the state and control weight matrices respectively, and Y p is the vector representation of the output expected value. In addition, the expected acceleration of the controlled vehicle must also meet the following hard constraints:

[0067] a min ≤a(t)≤a max

[0068] Among them, a min and a max is the expected minimum acceleration and the expected maximum acceleration. Therefore, the above two formulas of optimization performance index and hard constraint constitute a complete optimization goal, and by solving them, the control sequence of the expected acceleration of the controlled vehicle in the future is obtained. Since it is relatively complicated to directly solve these optimization performance indicators, by converting the problem into a matrix operation, it can be simplified to an easy-to-solve quadratic programming problem, so that the MPC controller can effectively perform rolling optimization in each step.

[0069]

[0070] Among them, H t =Θ T SΘ+W,G t =2(φξ-Y p )SΘ;

[0071] Use the quadratic programming (QP) solver to solve N c The control input increment within

[0072]

[0073] The first element of the incremental sequence is used as the actual input increment in the controller to achieve rolling optimization. Finally, feedback correction is performed. Since the state ξ(t) of the controlled vehicle is measurable, the ξ(t) measured at each sampling time point can be used to start the prediction and optimization of that time point. In this way, both prediction and optimization are based on the real-time feedback of the controlled vehicle, and feedback correction is naturally achieved.

[0074] Generation of throttle and brake calibration table

[0075] Nowadays, electric vehicles are mainly used in the development of autonomous driving systems. Therefore, a motor model needs to be built in Carsim. The maximum power of the motor P max The maximum torque of the motor is 125kW. max The purpose of generating the throttle and brake calibration table is to control the throttle or brake opening of the vehicle in real time according to the speed and acceleration values ​​of the vehicle during driving, so as to control the driving or braking of the vehicle. The throttle and brake calibration tables in this paper are generated by traditional methods, that is, different speed v and acceleration a curves are generated by different degrees of pedal pressure, and then these speed and acceleration curves are integrated to obtain the va curve.

[0076] Furthermore, in step five, joint simulation is performed based on Simulink and Carsim.

[0077] The present invention combines LQR and MPC algorithms to perform vehicle lateral and longitudinal trajectory tracking control, which not only ensures tracking accuracy and stability, but also makes the design simpler.

[0078] 3. Beneficial effects

[0079] Compared with the existing known technologies, the technical solution provided by the present invention has the following significant effects:

[0080] (1) The Dung Beetle Optimization (DBO) algorithm is used to determine the weight matrix parameters of the LQR controller. Compared with the LQR controller optimized by the Ant Lion (ALO) algorithm and the LQR controller with manually fixed weight parameters, it has more precise lateral control of the vehicle and higher accuracy in tracking the reference trajectory.

[0081] (2) The DBO-LQR and MPC strategies can be used to control the lateral and longitudinal trajectory tracking of the vehicle at the same time. The effect of tracking the quintic polynomial planning trajectory is good under three working conditions: urban road parking, urban road lane changing, and highway lane changing. The lateral error and heading deviation are both small. The proposed strategy can complete trajectory tracking under various working conditions and has high tracking accuracy while taking into account vehicle stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 This is a horizontal and vertical control flow chart of the intelligent vehicle trajectory tracking control based on the dung beetle optimization algorithm in the present invention;

[0083] Figure 2 is the vehicle dynamics model diagram;

[0084] Figure 3 Optimize algorithm flow chart for dung beetles;

[0085] Figure 4 Result graph for iterative optimization;

[0086] Figure 5 Comparison diagram for tracking S-shaped large curvature paths;

[0087] Figure 6 Comparison diagram of lateral error for tracking S-shaped path;

[0088] Figure 7 It is a comparison chart between expected speed and actual speed;

[0089] Figure 8 It is a comparison chart between planned lateral displacement and actual lateral displacement;

[0090] Fig. 9 It is a comparison chart of planned longitudinal displacement and actual longitudinal displacement;

[0091] Fig.10 This is a comparison chart of lateral tracking errors;

[0092] Fig.11 This is a comparison chart of heading deviation;

[0093] Fig.12 This is a comparison chart of the front wheel turning angle;

[0094] Fig.13 This is a comparison chart of yaw angular velocity. DETAILED DESCRIPTION

[0095] In order to further understand the content of the present invention, the present invention is described in detail in conjunction with the accompanying drawings and embodiments.

[0096] Example 1

[0097] This embodiment provides a smart vehicle trajectory tracking control method based on a dung beetle optimization algorithm.

[0098] The technical solution of the present invention is further elaborated in detail below in conjunction with specific embodiments. The parts of the present invention that are not described and disclosed in detail in the following embodiments should be understood as the prior art known or should be known to those skilled in the art.

[0099] like Figure 1 As shown in the figure, a trajectory tracking control method for an intelligent vehicle based on the dung beetle optimization algorithm is used to establish the lane change trajectory function of the controlled vehicle through a quintic polynomial, and the optimal lane change trajectory is obtained according to the lane change trajectory function, so as to realize efficient and accurate trajectory tracking of the intelligent vehicle. Then, a trajectory tracking error model of the controlled vehicle is established, and the LQR control algorithm is introduced to perform lateral adjustment on the controlled vehicle and the MPC controller is introduced to perform longitudinal control on the controlled vehicle, so as to adjust the displacement error of the controlled vehicle, effectively eliminate the cumulative error in the trajectory tracking of the controlled vehicle, ensure that the controlled vehicle travels according to the planned trajectory, improve the stability of trajectory tracking, and enable the vehicle to change lanes according to the optimal trajectory. Among them, the LQR controller introduces the dung beetle optimization algorithm to adjust its parameters. The LQR parameters optimized by the dung beetle optimization algorithm perform better on the tracking path than the LQR parameters optimized by the ant lion (ALO) algorithm and manually fixed. The vehicle trajectory has a higher degree of consistency with the planned path, and the lateral tracking error is significantly reduced. Finally, the MPC controller outputs throttle and brake control signals to execute the trajectory tracking control of the controlled vehicle and coordinate the controlled vehicle to change lanes smoothly.

[0100] Specifically, the following steps are included:

[0101] The real-time status information of the controlled vehicle is obtained by using radar and sensor devices. The status information includes the current displacement, speed, acceleration and yaw angle of the controlled vehicle, and the safety constraint conditions are established based on the real-time status information. In this embodiment, the reference trajectory is set using a quintic polynomial, which can be expressed as

[0102]

[0103] x(t) and y(t) represent the longitudinal and lateral displacements of the vehicle, and a0, a1, a2, a3, a4, a5, b0, b1, b2, b3, b4, b5 are unknown coefficients.

[0104] In addition, according to the urban road engineering design specifications, the reference trajectory needs to meet the constraints of the start and end points:

[0105]

[0106] Where x represents the longitudinal displacement, represents the longitudinal velocity, represents longitudinal acceleration, y represents lateral displacement, y′ represents lateral velocity, y″ represents lateral acceleration, T represents the time taken to travel from the starting point to the end point, and x end is the horizontal coordinate of the end point. By setting different restrictions, we can solve the unknown coefficients a0, a1, a2, a3, a4, a5, b0, b1, b2, b3, b4, b5 of the quintic polynomial. At the same time, convert y(x) into y(t) to facilitate the solution of the vertical coordinate:

[0107]

[0108] Where: θ p is the tangent angle of the desired trajectory, k p is the curvature of the desired trajectory, y p ′ is the desired lateral velocity, y p ″ is the expected lateral acceleration;

[0109] Then solve for the tangent angle θ of the desired trajectory p and curvature k p

[0110]

[0111] Finally, the reference trajectory also limits the range of acceleration:

[0112]

[0113] The dynamic model of the controlled vehicle is established, and the trajectory tracking error model is established based on the lane change trajectory function and the dynamic model. Specifically, in order to reduce the amount of calculation, it is assumed that the controlled vehicle is traveling on a flat road, the longitudinal and lateral coupling of tire forces, the load transfer of the front and rear axles are ignored, the left and right load transfer is not considered, and the influence of the lateral and longitudinal aerodynamics on the yaw characteristics of the controlled vehicle is ignored. The dynamic model of the controlled vehicle is established:

[0114]

[0115] in, is the lateral velocity, is the lateral acceleration, is the yaw angular acceleration, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, I z is the moment of inertia of the vehicle, and δ is the front tire slip angle.

[0116] set up u=δ, transform the above formula into a general form

[0117]

[0118] Where: X is the state variable matrix; A is the state variable coefficient matrix; B is the control variable coefficient matrix. u is the control variable. Therefore, by controlling δ f To achieve y and control.

[0119] The tracking error model is established based on the dynamic model and the objective function. Its goal is to select the appropriate control variable u, that is, to solve the objective function J = e rr 2 +u 2 exist The error between the current actual position and the planned trajectory point is found. The point closest to the planned discrete trajectory point and the actual position is called the matching point. The error between the actual position and the matching point is analyzed. The lateral error, the rate of change of the lateral error, the heading angle, and the rate of change of the heading angle error are used as state quantities to construct the error state space equation, that is, the trajectory tracking error model is:

[0120]

[0121] Among them, e d is the lateral displacement error, is the lateral velocity error, is the lateral acceleration error, is the yaw angle error, is the yaw rate error, is the yaw angular acceleration error, is the heading angular velocity, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, Iz is the moment of inertia of the vehicle, and δ is the front wheel steering angle.

[0122] according to Figure 2 Firstly, an optimal lane-changing trajectory is fitted using a quintic polynomial curve, and the planned trajectory displacement and planned speed are input into the controller. The longitudinal MPC controller is adjusted according to the error between the actual and planned speeds. The speed error and acceleration error obtained are output to the model through the throttle and brake calibration table. The lateral LQR controller with feedforward + feedback optimized by the dung beetle optimization algorithm is introduced to adjust the planned displacement error and output the front wheel steering angle to the motor model, thereby controlling the acceleration or braking of the vehicle in real time.

[0123] First, the design is based on the feedforward + feedback LQR control algorithm, the trajectory tracking error is optimized to obtain the front wheel angle of the controlled vehicle, and the controlled vehicle is adjusted laterally. In this embodiment, the feedforward + feedback LQR control algorithm adjusts the steering input according to the error between the discrete trajectory point and the actual position:

[0124] The forward Euler method and the midpoint Euler method are used to discretize the control quantity and state quantity, thereby obtaining the discrete state space equation:

[0125]

[0126] Where: X k , U k are the system state quantity and control quantity corresponding to time k respectively.

[0127] In order to take into account both tracking error and energy loss, the function can be defined:

[0128]

[0129] Therefore, the path lateral tracking problem is transformed into solving the minimum value of the cost function under the constraints.

[0130] The feedback control law of the LQR controller is:

[0131]

[0132] Among them, R is the input weight matrix, is the transpose of the control input matrix, P is the state covariance matrix at time k+1, A is the state transfer matrix, is the control input matrix, X k is the state vector at time k.

[0133] Introduce the horizontal and vertical error e rr , the front wheel steering angle δ is used as the control variable, and the optimal front wheel steering angle control law of the LQR controller can be obtained:

[0134] δ(k)=-Ke rr (k)

[0135] Among them, K = [k1 k2 k3 k4] is the optimal control law feedback matrix of the LQR controller.

[0136] When calculating the optimal front wheel steering angle of the LQR controller based on the trajectory tracking error model, simplifying the model will bring unavoidable steady-state errors to the controlled system. In order to eliminate the steady-state error, it is necessary to introduce an additional feedforward steering angle control variable δ f Make rr Converge to 0 as much as possible, then redefine the full state feedback control and error differential equation:

[0137]

[0138] K is the feedback gain obtained by the LQR control algorithm. When the control is stable:

[0139]

[0140] When the lateral error e d = 0, the wheel angle feedforward can be easily obtained:

[0141]

[0142] Among them, k is the feedback gain obtained by the LQR control algorithm, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, k3 is the control gain of the state weight matrix, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle.

[0143] The Dung Beetle Optimization Algorithm (DBO) is introduced to optimize the LQR weight matrix parameters. The two weight coefficients Q and R in LQR control represent the limiting effects on the system state quantity and control quantity, respectively, which affect the vehicle trajectory tracking effect and can be written in the following matrix form:

[0144] Q=diag[q1,q2,q3,q4],R=[r]

[0145] In general, the larger the value of the elements in the Q matrix, the faster the system responds. The larger the value of the elements in the R matrix, the greater the limit on the control quantity. Since the weight coefficient determination process of the LQR controller is complex and usually determined by experience, the dung beetle optimization algorithm is used to determine the three parameters q1, q3 and r of the diagonal of the weighted matrices Q and R. In order to ensure both driving stability and trajectory tracking capability, the lateral distance deviation e is used. d , heading angle deviation The weighted square sum of the front wheel angle δ changes is used as the fitness function of the dung beetle algorithm optimization:

[0146]

[0147] Since the emphasis is on the effect of trajectory tracking, the weighting coefficients ψ1 = 0.4, ψ2 = 0.35, and ψ3 = 0.25 are set. The maximum number of iterations is set to 30, and the initial number of individuals in the dung beetle population is 30. The optimization algorithm flow chart is as follows: Figure 3 , the iterative optimization results are as follows Figure 4 As shown in Figure 2, in order to verify the feasibility and superiority of DBO optimized LQR controller in path tracking, a large curvature S-shaped path is selected, and the ant lion optimizer (ALO) algorithm is used to optimize the LQR controller, the artificial fixed weight coefficient LQR controller and the DBO algorithm to optimize the LQR controller to make the vehicle track the reference path. The results are shown in Figure 2. Figure 5 , 6. By Figure 5 ,6 shows that: compared with the ALO algorithm optimized LQR controller and the artificial fixed weight coefficient LQR controller, the dung beetle optimization algorithm optimized LQR controller performs better in tracking the reference path, and the vehicle trajectory is more consistent with the reference path; the lateral tracking error of the ALO algorithm optimized LQR controller is maintained within 0.25m, while the lateral tracking error of the dung beetle optimization algorithm optimized LQR controller is significantly reduced and maintained within 0.16m.

[0148] Then, through the controlled vehicle's own parameters (as shown in Table 1), and the longitudinal speed v x The two parameters A and B are obtained, and the two parameters Q and R of the LQR weight matrix are optimized by the Dung Beetle Optimization Algorithm (DBO), and the feedback gain k is obtained by the LQR control algorithm; then the error e between the current state and the planned state is obtained by the controlled vehicle's own state, planned speed and current longitudinal speed. rr and road curvature k; finally, through the feedback gain, current longitudinal speed, k and e rr The feedforward control quantity δ is obtained f , and then the feedback control amount δ is obtained.

[0149] Table 1 The parameters of the controlled vehicle

[0150]

[0151] The real-time state information, optimal trajectory information and optimized trajectory tracking error are designed to be transmitted to the MPC controller, and finally, the parameters of the MPC controller are output. In this embodiment, the longitudinal displacement error and the longitudinal velocity error are used as the input of the MPC controller. Specifically, the longitudinal model predictive control (MPC) is designed to achieve velocity tracking, and the longitudinal velocity is used to connect the lateral and longitudinal controllers, and the speed and steering of the vehicle are controlled at the same time to achieve trajectory tracking control of the controlled vehicle.

[0152] Assuming that the vehicle's yaw motion is small, we have:

[0153]

[0154] In the formula, a x is the longitudinal acceleration, is the longitudinal acceleration;

[0155] The lower controller is established by approximating the first-order inertial system, as shown in Eq.

[0156]

[0157] Where τ is the inertia time constant; a is the longitudinal acceleration; is the rate of change of longitudinal acceleration; a p is the expected longitudinal acceleration. The vehicle longitudinal motion equation can be described as follows, where v is the vehicle longitudinal velocity.

[0158]

[0159] Take the state quantity ξ=[v,a,] T , control quantity u=a p , output O(t), the above formula is described as the state space expression

[0160]

[0161] t, t+1, and T are the current sampling time point, the next cycle sampling time point, and the sampling cycle, respectively.

[0162] Write the prediction equation in matrix form, as follows:

[0163] Y(t)=φξ(t)+ΘU(t)

[0164] In the formula N p ,N cThey are respectively the prediction time domain and the control time domain. After establishing the model for predicting the state of the controlled vehicle, the next step of rolling optimization is carried out. The state optimization problem at time point t can be specifically expressed as: determine the m control quantities u(t), u(t+1), u(t+2), ..., u(t+m-1) from this time point, so that the state of the controlled vehicle under its influence at the next j time points is close to the expected state to the maximum extent, preventing the vehicle speed from changing sharply. Therefore, the optimization performance index can be expressed in the following vector form:

[0165]

[0166] Among them, S and W are the state and control weight matrices respectively, and Y p is the vector representation of the output expected value. In addition, the expected acceleration of the controlled vehicle must also meet the following hard constraints:

[0167] a min ≤a(t)≤a max

[0168] Among them, a min and a max is the expected minimum acceleration and the expected maximum acceleration. Therefore, the above two formulas of optimization performance index and hard constraint constitute a complete optimization goal, and by solving them, the control sequence of the expected acceleration of the controlled vehicle in the future is obtained. Since it is relatively complicated to directly solve these optimization performance indicators, by converting the problem into a matrix operation, it can be simplified to an easy-to-solve quadratic programming problem, so that the MPC controller can effectively perform rolling optimization in each step.

[0169]

[0170] Among them, H t =Θ T SΘ+W,G t =2(φξ-Y p )SΘ;

[0171] Use the quadratic programming (QP) solver to solve N c The control input increment within

[0172]

[0173] The first element of the incremental sequence is used as the actual input increment in the controller to achieve rolling optimization. Finally, feedback correction is performed. Since the state ξ(t) of the controlled vehicle is measurable, the ξ(t) measured at each sampling time point can be used to start the prediction and optimization of that time point. In this way, both prediction and optimization are based on the real-time feedback of the controlled vehicle, and feedback correction is naturally achieved.

[0174] According to the MPC parameters, the throttle and brake control signals are output to perform trajectory tracking control of the controlled vehicle, that is, the speed error and acceleration error are input into the throttle and brake calibration tables to control the front wheel steering angle and driving braking force of the vehicle.

[0175] The dung beetle optimization algorithm is used to determine the LQR weight coefficient, which takes into account the distance deviation, heading angle deviation and front wheel turning angle to achieve accurate lateral tracking. For longitudinal control, model predictive control (MPC) is used to track speed and position, which can make real-time adjustments according to trajectory deviation, significantly improving the control accuracy and stability of trajectory tracking.

[0176] Simulation and result analysis

[0177] The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm described in Example 1 adopts a method for determining the LQR weight coefficient based on the dung beetle optimization algorithm, while taking into account the distance deviation, heading angle deviation and front wheel turning angle to achieve accurate lateral tracking. The longitudinal control adopts model predictive control (MPC) to track the speed and position, and can be adjusted in real time according to the trajectory deviation, which is convenient for promotion and application.

[0178] The Carsim-Matlab simulation platform was built using Carsim 2019.1 and Matlab 2020b. A C-class car was selected in Carsim, and the vehicle parameters were set as shown in Table 1. The simulation conditions were set as urban road parking, urban road lane change, and highway lane change. The simulation test time was 16s, and the specific working condition parameters were shown in Table 2. The vehicle controller was built in Matlab / Simulink, and the control module calculated the control law for the vehicle throttle, brake, and steering wheel, and passed the control law to the vehicle model in CarSim. The DBO-LQR and MPC control algorithms were used to track the lateral and longitudinal trajectories of the vehicle under three working conditions. The results are shown in Table 2. Figure 7 , Figure 8 , Fig. 9 , Fig.10 , Fig.11 , Fig.12 , Fig.13 .

[0179] Table 2 Working condition settings

[0180]

[0181] Depend on Figure 7 , Figure 8 , Fig. 9 , Fig.10 , Fig.11 , Fig.12 , Fig.13It can be seen that: under the urban road parking condition, the actual speed of the vehicle in the starting stage has a little overshoot compared with the expected speed, the actual lateral and longitudinal displacement tracking planning of the vehicle has good results, the maximum lateral tracking deviation is less than 0.010m, and the heading deviation is within 0.0250rad, indicating that the controller has high-precision tracking ability and good lateral stability; the front wheel angle and yaw rate do not show obvious jitter, and the change is stable, indicating that the controller has good robustness. Under the highway lane change condition, the actual speed of the vehicle in the 0-2s starting stage has a little overshoot compared with the expected speed, which is smaller than the urban road parking condition; the actual lateral and longitudinal displacement tracking planning of the vehicle has good results, the lateral error of the tracking trajectory is less than 0.008m, and the heading deviation is within 0.0008rad, indicating that the controller has higher tracking accuracy under this condition than the urban road parking condition; the front wheel angle and yaw rate are smaller than the urban road parking condition, and the change is stable. Under the lane-changing condition on urban roads, the actual speed of the vehicle in the 0-2s starting phase has a small deviation from the expected speed. The actual lateral and longitudinal displacement tracking planning of the vehicle has good effects. The lateral error of the tracking trajectory is less than 0.006m, and the heading deviation is within 0.0005rad, indicating that the controller has higher tracking accuracy under the lane-changing condition on urban roads. The front wheel turning angle and yaw angular velocity increase compared with the lane-changing condition on highways, but the changes remain stable.

[0182] The present invention and its embodiments are described schematically above, and the description is not restrictive. The drawings show only one embodiment of the present invention, and the actual structure is not limited thereto. Therefore, if a person skilled in the art is inspired by it and designs a structural method and an embodiment similar to the technical solution without creativity without departing from the purpose of the invention, they shall all fall within the protection scope of the present invention.

Claims

1. A smart vehicle trajectory tracking control method based on a dung beetle optimization algorithm, characterized in that: The steps are: Step 1: obtain the real-time status information of the controlled vehicle, establish safety constraints based on the real-time status information, establish a lane-changing trajectory function of the controlled vehicle based on multiple polynomials, and obtain the optimal lane-changing trajectory related parameters according to the lane-changing trajectory function; Step 2: Establish a dynamic model of the controlled vehicle, and establish a trajectory tracking error model based on the lane change trajectory function and the dynamic model; Step 3: Introduce the dung beetle optimization algorithm to optimize the LQR weight matrix parameters, optimize the trajectory tracking error according to the feedforward + feedback LQR control algorithm to obtain the front wheel angle of the controlled vehicle, and make lateral adjustments to the controlled vehicle; Step 4: Design longitudinal model-based predictive control to achieve speed tracking, use the longitudinal speed to connect the lateral and longitudinal controllers, and control the speed and steering of the vehicle at the same time to achieve trajectory tracking control of the controlled vehicle.

2. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 1 is characterized in that: In step 1, the real-time status information includes the current displacement, velocity, acceleration and yaw angle of the controlled vehicle; the safety constraints are: Where: x represents the longitudinal displacement, represents the longitudinal velocity, represents longitudinal acceleration, y represents lateral displacement, y′ represents lateral velocity, y″ represents lateral acceleration, T represents the time taken to travel from the starting point to the end point, and x end is the horizontal coordinate of the end point.

3. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 1 is characterized in that: In step 1, a lane-changing trajectory function of the controlled vehicle is established based on a quintic polynomial, and the optimal lane-changing trajectory-related parameters are obtained according to the lane-changing trajectory function, wherein the lane-changing trajectory function of the controlled vehicle is: Where: x(t) and y(t) represent the longitudinal and lateral displacements of the vehicle, a0, a1, a2, a3, a4, a5, b0, b1, b2, b3, b4, b5 are unknown coefficients, and t represents the simulation time; The optimal lane-changing trajectory is fitted using the lane-changing trajectory function of the controlled vehicle, where the tangent angle θ of the optimal lane-changing trajectory is p and curvature k p They are: Where: θ p is the tangent angle of the desired trajectory, k p is the curvature of the desired trajectory, y′ p is the expected lateral velocity, y″ p is the expected lateral acceleration; The acceleration range of the lane change trajectory is limited to:

4. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 1 is characterized in that: In step 2, the dynamic model of the controlled vehicle is established as: in, is the lateral velocity, is the yaw angular velocity, is the lateral acceleration, is the yaw angular acceleration, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, I z is the moment of inertia of the vehicle, δ is the front tire slip angle; The established trajectory tracking error model is: Among them, e d is the lateral displacement error, is the lateral velocity error, is the lateral acceleration error, is the yaw angle error, is the yaw rate error, is the yaw angular acceleration error, is the heading angular velocity, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle, I z is the moment of inertia of the vehicle, and δ is the front wheel steering angle control variable.

5. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 1 is characterized in that: Step 3 is as follows: The LQR weight matrix parameters include two weight coefficients Q and R, which represent the limiting effects on the system state quantity and control quantity respectively, and are written in the following matrix form: Q=diag[q1,q2,q3,q4],R=[r] The three parameters q1, q3 and r of the diagonal of the weighted matrix Q and R are determined by the dung beetle optimization algorithm, and the lateral distance deviation e is determined by d , heading angle deviation The weighted square sum of the front wheel angle δ and the front wheel angle δ change Δδ is used as the fitness function optimized by the dung beetle algorithm: Wherein, weight coefficients ψ1 = 0.4, ψ2 = 0.35, ψ3 = 0.25; The feedforward + feedback LQR control algorithm adjusts the steering input according to the error between the discrete trajectory points and the true position: The forward Euler method and the midpoint Euler method are used to discretize the control quantity and state quantity, thereby obtaining the discrete state space equation: Where: X k , U k are the system state quantity and control quantity corresponding to time k respectively; In order to take into account both tracking error and energy loss, the function can be defined: The feedback control law of the LQR controller is: Among them, R is the input weight matrix, is the transpose of the control input matrix, P is the state covariance matrix at time k+1, A is the state transfer matrix, is the control input matrix, X k is the state vector at time k, Introduce the horizontal and vertical error e rr , the front wheel steering angle δ is used as the control variable, and the optimal front wheel steering angle control law of the LQR controller can be obtained: δ(k)=-Ke rr (k) Where, K = [k1 k2 k3 k4], is the optimal control law feedback matrix of the LQR controller; When calculating the optimal front wheel steering angle of the LQR controller based on the trajectory tracking error model, simplifying the model will bring unavoidable steady-state errors to the controlled system. In order to eliminate the steady-state error, it is necessary to introduce an additional feedforward steering angle control variable δ f Make rr Converge to 0 as much as possible, then redefine the full state feedback control and error differential equation: K is the feedback gain obtained by the LQR control algorithm. When the control is stable: When the lateral error e d =0, it is easy to get the wheel angle feedforward: Among them, k is the feedback gain obtained by the LQR control algorithm, i is the distance from the center of mass of the controlled vehicle to the front axle, b is the distance from the center of mass of the controlled vehicle to the rear axle, k3 is the control gain of the state weight matrix, C αf is the cornering stiffness of the front tire, C αr is the cornering stiffness of the rear tire, m is the mass of the controlled vehicle, v x is the longitudinal velocity of the controlled vehicle.

6. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 5 is characterized in that: Set the weighting coefficients ψ1=0.4, ψ2=0.35, ψ3=0.25, set the maximum number of iterations to 30, and the initial number of individuals in the dung beetle population to 30.

7. The intelligent vehicle trajectory tracking control method based on the improved dung beetle algorithm according to claim 1 is characterized by: Step 4 is as follows: Assuming that the vehicle's yaw motion is small, we have: In the formula, a x is the longitudinal acceleration, is the longitudinal acceleration; The lower controller is established by approximating the first-order inertial system, as shown in Eq. Where τ is the inertia time constant; a is the longitudinal acceleration, is the rate of change of longitudinal acceleration, a p is the expected longitudinal acceleration; The vehicle longitudinal motion equation can be described as follows, where v is the vehicle longitudinal velocity: In the formula, t, t+1, and T are the current sampling time point, the next cycle sampling time point, and the sampling period, respectively; Take the state quantity ξ=[v,a,] T , control quantity u=a p , output O(t), the above formula is described as the state space expression The model for predicting the state of the controlled vehicle is written in matrix form, as follows: Y(t)=φξ(t)+ΘU(t) In the formula, N p ,N c They are prediction time domain and control time domain respectively; After building a model to predict the state of the controlled vehicle, the next step is rolling optimization: The state optimization problem at time point t can be specifically described as follows: determine m control quantities u(t), u(t+1), u(t+2), ..., u(t+m-1) from this time point, so that the state of the controlled vehicle under its influence at the next j time points will be close to the expected state to the maximum extent possible, and prevent the vehicle speed from changing sharply; the optimization performance index is: Among them, S and W are the state and control weight matrices respectively, and Y p is the vector representation of the output expected value; In addition, the desired acceleration of the controlled vehicle must meet the following hard constraints: a min ≤a(t)≤a max Among them, a min and a max are the expected minimum acceleration and the expected maximum acceleration; Therefore, the above two formulas of optimization performance index and hard constraint constitute a complete optimization goal, and they are solved to obtain the control sequence of the expected acceleration of the controlled vehicle at the future moment.

8. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 7 is characterized in that: In step 4: the method for solving the optimization objective is: By converting the problem into a matrix operation, it can be simplified to an easily solvable quadratic programming problem, allowing the MPC controller to effectively perform rolling optimization at each step. among themH t =Θ T SΘ+W,G t =2(φξ-Y p )SΘ; Use the quadratic programming solver to solve and get N c The control input increment within The first element of the increment sequence is used as the actual input increment in the controller to achieve rolling optimization.

9. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to claim 8 is characterized in that: After the rolling optimization is realized, feedback correction is also performed. Since the state ξ(t) of the controlled vehicle is measurable, the ξ(t) measured at each sampling time point can be used to start the prediction and optimization of that time point. In this way, the prediction and optimization are based on the real-time feedback of the controlled vehicle, and feedback correction can be naturally realized.

10. The intelligent vehicle trajectory tracking control method based on the dung beetle optimization algorithm according to any one of claims 1 to 9, characterized in that: It also includes step five: performing joint simulation based on Simulink and Carsim to verify the tracking accuracy of the designed controller under different working conditions.

Citation Information

Patent Citations

  • Magnetic-levitation train operation optimization method and system based on improved dung beetle optimization algorithm

    CN117034459A

  • Wheeled robot advancing state estimation method based on improved Kalman filtering

    CN117406607A

  • Parameter-adaptive intelligent patrol car high-precision trajectory tracking control method

    CN117518779A

  • Identification of data related to another data set within a data set described based on plural ontologies

    US20200117662A1

Cited By

  • Trajectory tracking control method for electric vehicle

    CN119370127A

  • Three-dimensional route planning method for fixed-wing unmanned aerial vehicle in complex mountain land

    CN119472769A

  • Vehicle cluster cooperative positioning method based on MDS-IDBO algorithm

    CN119676635A

  • Vehicle lane changing trajectory tracking error elimination method and device and storage medium

    CN120096601A

  • Trajectory tracking control algorithm related to unmanned vehicle sensor fusion

    CN120122660A