Vehicle transverse and longitudinal trajectory tracking collaborative optimization control method and system

Abstract

The invention discloses a vehicle transverse and longitudinal trajectory tracking collaborative optimization control method and system. The method comprises the following steps: establishing a vehicle monorail model and an error calculation module; respectively designing a transverse MPC controller, a longitudinal MPC upper controller and a lower controller based on an acceleration-driving force inverse dynamics model according to the vehicle monorail model; carrying out normalization processing on the transverse error and the yaw angle error which are calculated by the error calculation module, and taking a normalization processing result as a fitness function of a pelican algorithm; establishing a Latin-pelican optimization algorithm fusing population initialization of sine-Latin hypercube sampling, prey position updating of a grade system and a dynamic random search strategy; a fitness function and a Latin-pelican algorithm are utilized to optimize a transverse MPC controller and a longitudinal MPC upper controller respectively, physical mapping of control quantity is achieved in combination with a lower controller based on an acceleration-driving force inverse dynamics model, optimal weight matrix parameters are obtained, and transverse and longitudinal joint trajectory tracking control of the vehicle is completed.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of vehicle trajectory tracking technology, in particular to a vehicle lateral and longitudinal trajectory tracking collaborative optimization control method and system based on LPOA-MPC. BACKGROUND

[0002] Trajectory tracking control is one of the core technologies of vehicle motion control, and its goal is to enable the vehicle to accurately and timely follow the reference trajectory on the premise of ensuring the driving stability of the vehicle and the riding experience of the passengers based on the current positioning and state information combined with the planning and decision system. According to different control objectives, the trajectory tracking control of the vehicle can be divided into lateral control, longitudinal control and lateral-longitudinal combined control. The longitudinal control of the vehicle controls the speed and the distance between the front and rear vehicles or obstacles by controlling the brake system and the engine speed of the vehicle; the lateral control controls the vehicle to keep the desired driving route by controlling the front wheel angle; the lateral-longitudinal combined control comprehensively considers the lateral and longitudinal control to achieve overall control. Model predictive control is widely used in non-linear and multi-constrained control tasks due to its optimization characteristics based on the system dynamics model. However, the performance of the MPC controller depends largely on the reasonable selection of the weight matrix parameters, and the weight matrix plays a role in balancing different performance indicators in the optimization process of the MPC. Therefore, reasonable parameter setting is crucial to ensure the stability and tracking accuracy of the controller under various complex working conditions. The Pelican Optimization Algorithm (POA) is a new type of swarm intelligence optimization method proposed by simulating the hunting strategy and behavior of pelicans in the hunting process in nature, regarding the position of the pelicans in the population as candidate solutions and regarding the objective function value as the quality of food, aiming to find the optimal solution by simulating the foraging behavior of pelicans. POA is very effective in exploration compared with other intelligent algorithms, but it has problems of convergence speed decay and weak local extremum escape ability in complex multi-modal optimization problems [8] . [9] . SUMMARY

[0003] The present application aims to provide a vehicle lateral and longitudinal trajectory tracking collaborative optimization control method and system to overcome the problems existing in the prior art. The present application expands the application scenarios of the POA algorithm, and combines LPOA with the hierarchical collaborative optimization theory to establish a weight matrix parameter collaborative optimization vehicle trajectory tracking control model of the lateral and longitudinal model predictive control (MPC), thereby improving the control performance of the trajectory tracking controller.

[0004] To achieve the above-mentioned purpose, the present application adopts the following technical solutions: The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method comprises the following steps: Step 1: Establish a vehicle single-track model and an error calculation module; Step 2: Design a lateral MPC controller, a longitudinal MPC upper controller and a lower controller based on an acceleration-driving force inverse dynamics model according to the vehicle single-track model established in step 1; Step 3: Obtain an initial pelican algorithm; Step 4: Normalize the lateral error and the yaw angle error calculated by the error calculation module established in step 1, and take the normalized results as the fitness function of the pelican algorithm in step 3; Step 5: Improve the pelican algorithm in step 3 to obtain a Latin-pelican optimization algorithm that integrates sine-Latin hypercube sampling population initialization, hierarchical prey position updating and dynamic random search strategy; Step 6: Use the fitness function obtained in step 4 and the Latin-pelican algorithm proposed in step 5 to optimize the lateral MPC controller and the longitudinal MPC upper controller designed in step 2, and combine the lower controller based on the acceleration-driving force inverse dynamics model to realize physical mapping of the control quantity, finally obtain the optimal weight matrix parameter, and complete the lateral and longitudinal joint trajectory tracking control of the vehicle.

[0005] Further, the vehicle single-track model and the error calculation module in step 1 are as follows: (1) (2) (3) (4) (5) (6) (7) Among them, formula (1) is the balance equation of the vehicle on the x axis according to Newton's second law; formula (2) is the balance equation of the vehicle on the y axis according to Newton's second law; formula (3) is the balance equation of the vehicle on the z axis according to Newton's second law; formula (4) is the established vehicle single-track model; formula (5) is the calculation formula of the vehicle lateral deviation, formula (6) is the calculation formula of the vehicle yaw angle deviation, and formula (7) is the calculation formula of the vehicle longitudinal speed error, wherein, a , b are the distances from the center of mass to the front and rear axes, m is the vehicle body mass, F xf，F xr for front and rear wheels x force in the direction ，F yf ，F yr for front and rear wheels y force in the direction, C cf , C cr , C lf , C lr longitudinal stiffness and cornering stiffness of the front and rear tires of the vehicle respectively, I z moment of inertia of the vehicle about z axis, α tire cornering angle, μ friction coefficient, F z vertical load, s slip ratio, C l longitudinal stiffness of the tire, C c cornering stiffness of the tire, yaw angle, φ r angle between the road tangent and the earth coordinate system X axis, v r longitudinal reference speed, e v longitudinal speed error, v s speed of the vehicle along the trajectory, δ f front wheel steering angle, Y , X actual lateral and longitudinal positions of the vehicle respectively, Y t , X t reference lateral and longitudinal positions of the vehicle respectively.

[0006] Further, the design lateral MPC controller is as follows: The state equation of the single-track model of the vehicle is , and the control variable is u = δ f , and the state variable is After linearization of the single-track model of the vehicle, the following linear time-varying equation is obtained:

[0007] In the formula, for f right ξ The Jacobian matrix of ξ ( t ) is the MPC modeling system t The state quantity at the moment, u ( t ) is the system t Time input, for f right u The Jacobian matrix of , after discretization, obtains the state space expression as follows:

[0008] Where, ξ ( k ) is the current state matrix, ξ ( k+1 ) is the state matrix at the next moment, A(k) is the state transition matrix, B(k) is the control input matrix, u ( k ) is the input matrix, I is the sampling time, A(k)=I+TA(t) , I is the identity matrix, B (k)=TB(t) , set the output of the system to η =[ Y ] T , and discretize it to get the output state space expression as follows:

[0009] Where, ; The output expression of the MPC modeling system is as follows:

[0010] Where Y(k) represents the system output sequence in the prediction time domain, is the state prediction matrix, is the control increment prediction matrix, △U(k) represents the control increment in the prediction time domain Based on the above MPC modeling system, the performance indicators of the lateral MPC controller are as follows:

[0011] Where, η ( t + i | t )for t Predicted at all timest+i System output at any moment, ρ is the weight coefficient, ε is the relaxation factor, η ref is the system status reference value; The design of the longitudinal MPC upper controller is as follows: The longitudinal control is simplified to an integral and first-order inertial system, which can be expressed as:

[0012] Where, K is the system gain, τ is the time constant, a is the acceleration, v For speed, a des is the reference acceleration; According to the above simplified integral and first-order inertial system, the continuous system state space equation of the vehicle longitudinal motion is expressed as:

[0013] In the formula, the state vector x =[ va ] T , u = a des System input for longitudinal MPC modeling, A 、 B The matrix is ​​as follows: ; The state space equation of the system is discretized to obtain the discrete system state equation as follows:

[0014] Where, k is the current sampling moment, T s is the sampling period, A k 、 B k The matrix is ​​as follows:

[0015] The vehicle speed v As the output quantity of the longitudinal MPC modeling system, the output equation is expressed as:

[0016] Where, x ( k ) The current state quantity, y ( kcurrent time output, C = [1 0]; The control objective of the longitudinal MPC modeling system is the speed tracking accuracy, and the performance index of the longitudinal MPC controller is defined as follows:

[0017] wherein, t -1 is the last sampling time, y p (k+i|k) is the control output prediction value, y ref (k+i|k) is the control output reference value, (k+i|k) is the state at the time t+1 predicted according to the sampling information at the time t, k k+i is the state at the time t+1 predicted according to the sampling information at the time t, Δu(k+i) is the control input increment at the time t+1, k+i Q , R is the weight matrix; The design is based on an acceleration-driving force inverse dynamics model lower controller, specifically: The lower controller is established based on an acceleration-driving force inverse dynamics model, including control switching logic design of driving and braking and actuator mapping. The control switching logic of driving and braking meets the following key elements: (1) driving and braking cannot be performed simultaneously; (2) driving and braking cannot be frequently switched, and the critical acceleration a 0 is:

[0018] A vehicle speed and road adhesion coefficient correction factor is introduced, and the specific formula is as follows:

[0019] wherein, F α0 is the driving force when the throttle opening is 0, F is the total resistance during vehicle travel, including the rolling resistance and air resistance of the vehicle, k v is the vehicle speed correction factor, which is used to suppress the nonlinear influence of air resistance at high speed, k μ is the road adhesion coefficient correction factor, which is increased at low adhesion coefficient to avoid slipping, and the buffer acceleration q = 0.05 m / s 2 is introduced to avoid frequent switching between driving and braking, specifically as follows: . ​​

[0020] Furthermore, the Pelican algorithm formula in step 3 is as follows:

[0021]

[0022] Where, X i is the position of the ith solution, Dim is a dimensional space, UB j Indicates the j The upper bound of dimension, LB j Indicates the j The lower bound of dimension, N is the number of candidate solutions, rand Is a random number between [0,1]; The pelican begins to move towards the prey's location. The position of the pelican during the approach phase is updated as follows:

[0023]

[0024] Where, is the updated position in the first phase, is a random number with a value range of (0,1). I is a random number, and its value range is a random integer value in [1,2]. P j For prey in j The location of the dimension, F p is the target fitness value of the prey; After the pelican reaches the water surface, it spreads its wings above the water surface and collects the prey in its throat pouch. The position update during the skimming phase is as follows:

[0025]

[0026] Where, β is a random number with a value range of [0,1]. R is a random number, and its value range is a random integer value in [1,2]. t is the current iteration number, T 1 is the maximum number of iterations, is the updated position in the second phase, is the target fitness value of the new position of the i-th pelican after the second phase update.

[0027] Furthermore, the fitness function in step 4 is specifically: Taking into account the influence of the two error quantities, lateral error and yaw angle error, the two error quantities are integrated into a comprehensive evaluation index through weighted combination, as follows:

[0028]

[0029]

[0030] Where, is the vehicle yaw angle, is the reference yaw angle, 、 are the maximum and minimum values ​​of the yaw angle error, is the yaw angle error, y e is the lateral error, y is the actual lateral position of the vehicle, y ref is the lateral position of the reference trajectory, y e_max 、 y e_min are the maximum and minimum values ​​of the lateral error, w is the weighted adjustment factor.

[0031] The comprehensive evaluation index is brought into the ITAE evaluation index as the objective function to guide the design or adjustment of the controller. The specific mathematical expression is as follows: .

[0032] Furthermore, the population initialization of the sine-Latin hypercube sampling, the prey position update of the hierarchy, and the Latin-Pelican optimization algorithm of the dynamic random search strategy specifically include the following three mechanisms: Population initialization based on sine-Latin hypercube sampling; Hierarchical prey position updates; Dynamic random search strategy.

[0033] Furthermore, the population initialization based on sine-Latin hypercube sampling is as follows:

[0034] Where: X i After the population is initialized i Individuals, ls i is the corresponding LHS sequence, p is a random number with a value range of [0,1], which is used to control the randomness of initialization; The prey position update of the hierarchy system, in particular:

[0035]

[0036]

[0037] In the formula, D α , D β and D δ is ω the relative distance between the pelicans, α , β and δ the relative distance between the pelicans, X 1, X 2 and X 3 correspond to α , β and δ the pelican positions, X is ω the position of the pelican, C 1, C 2, C 3 is a random disturbance, A 1, A 2, A 3 is a random variable, X F is the updated prey position.

[0038] Further, the dynamic random search strategy, in particular includes: 1) Set the maximum number of iterations, initial step size and stopping criterion, initialize the current point, current point function value, optimal point and optimal value; 2) Reset the counter and start a new round of search; 3) Randomly generate a disturbance vector within the current step size range; 4) Calculate the function value after adding the disturbance vector to the current point; 5) If the function value is better than the optimal value, update the optimal point and optimal value, and continue the next disturbance search; 6) If the function value is not better than the optimal value, but still better than the current point function value, update the current point and current function value, and continue the next disturbance search; 7) If neither of the above two conditions is met, calculate the function value when the disturbance vector takes the opposite direction; 8) If the reverse disturbance function value is better than the optimal value, update the optimal point and optimal value, and continue the next disturbance search; 9) If the reverse disturbance function value is not better than the optimal value, but the reverse disturbance function value is better than the current point function value, update the current point and the current function value, the counter is added by one, and the next disturbance search is continued; 10) When the counter reaches the maximum number of iterations, the step size is reduced to half of the original, and the next round of search is started; 11) Determine whether the stopping criterion is met, if yes, end, otherwise repeat the above process.

[0039] Further, by using the fitness function obtained in step 4 and the Latin-Flamingo algorithm proposed in step 5, the lateral MPC controller and the longitudinal MPC upper controller designed in step 2 are optimized respectively, and the lower controller based on the acceleration-driving force inverse dynamics model is combined to realize the physical mapping of the control quantity, and finally the optimal weight matrix parameters are obtained, completing the lateral and longitudinal joint trajectory tracking control of the vehicle, specifically: After the Flamingo population iteration is completed, the fitness sizes of the optimized lateral MPC controller and longitudinal MPC upper controller are calculated respectively, the optimal individual is selected, the position of the optimal individual is assigned to the weight matrix of the lateral MPC controller and the longitudinal MPC upper controller respectively, and then the optimized longitudinal MPC upper controller and the lower controller based on the acceleration-driving force inverse dynamics model are combined to realize the lateral and longitudinal joint trajectory tracking control of the vehicle.

[0040] The vehicle lateral and longitudinal trajectory tracking collaborative optimization control system comprises: The first module is used for establishing a vehicle single-track model and an error calculation module; The second module is used for designing a lateral MPC controller, a longitudinal MPC upper controller and a lower controller based on an acceleration-driving force inverse dynamics model according to the vehicle single-track model; The third module is used for obtaining an initial Flamingo algorithm; The fourth module is used for normalizing the lateral error and the yaw angle error calculated by the error calculation module, and taking the normalization result as the fitness function of the Flamingo algorithm; The fifth module is used for improving the Flamingo algorithm to obtain a Latin-Flamingo optimization algorithm which fuses the initial population of the sine-Latin hypercube sampling, the prey position update of the hierarchy and the dynamic random search strategy; The sixth module is used for optimizing the lateral MPC controller and the longitudinal MPC upper controller by using the fitness function and the Latin-Flamingo algorithm, and combining the lower controller based on the acceleration-driving force inverse dynamics model to realize the physical mapping of the control quantity, and finally obtaining the optimal weight matrix parameters to complete the lateral and longitudinal joint trajectory tracking control of the vehicle.

[0041] Compared with the prior art, the present application has the following beneficial technical effects: The present application proposes a Latin Pelican Optimization Algorithm (LPOA) that fuses multiple mechanisms. On the basis of the original Pelican Optimization Algorithm, multiple mechanisms are fused. Population initialization is performed by fusing sinusoidal-Latin hypercube sampling, so that the distribution of pelican individuals is more uniform, the entire population can cover the search blind area to the greatest extent, and the diversity of pelican individuals is increased, thereby enhancing the optimization ability of the algorithm. In the prey position updating stage, the prey position updating of the hierarchical system is fused, which solves the slow convergence of POA. In the approach prey stage and the skim over the water stage, a dynamic random search strategy is integrated, which solves the problem that POA is prone to fall into local extreme points, and enhances the global search ability and expands the application scenarios of POA. At the same time, the LPOA is combined with the hierarchical collaborative optimization theory to establish a vehicle trajectory tracking control model for collaborative optimization of the weight matrix parameters of the horizontal and vertical model predictive control (MPC). The Latin Pelican Optimization Algorithm fusing multiple mechanisms is used to adaptively optimize the weight matrix of the horizontal and vertical MPC controllers, thereby improving the control performance of the trajectory tracking controller. The present application has better trajectory tracking effect under different working conditions. BRIEF DESCRIPTION OF DRAWINGS

[0042] The accompanying drawings are included to provide a further understanding of the present application, and constitute a part of the present application. The illustrative embodiments of the present application and their description serve to explain the present application. They do not, however, limit the present application.

[0043] Figure 1 is a flowchart of the present application; Figure 2 is a flowchart of the vehicle horizontal and vertical trajectory tracking collaborative optimization control method based on LPOA-MPC involved in the present application; μ is a comparison chart of the optimization ability of the Latin Pelican Optimization Algorithm and the Pelican Optimization Algorithm designed in the present application, wherein (a) is the Schwefel 2.21 function; (b) is the Schwefel 2.26 function, (c) is the Rastrigin function, (d) is the Ackley function, and (e) is the Shekel function; φ is a horizontal error and yaw angle error chart of vehicle trajectory tracking under different working conditions in the present application, wherein (a) is a trajectory comparison; (b) is a horizontal error comparison curve, (c) is a yaw angle error comparison curve, (d) is a yaw angle velocity error comparison curve, (e) is a speed comparison curve, and (f) is a speed error comparison curve. DETAILED DESCRIPTION

[0044] The present application will be described in further detail below with reference to the accompanying drawings: Embodiment one Reference , δ For the flowchart of the present application, the present application optimizes and improves the basic pelican search algorithm, and proposes a Latin-pelican optimization algorithm (LPOA) which fuses the population initialization of sine-Latin hypercube sampling, the prey position update of hierarchy system and the dynamic random search strategy. δ , which is applied to the MPC controller in the optimization of vehicle lateral and longitudinal trajectory tracking control. First, a single-track model of the vehicle is established and a fitness function suitable for trajectory tracking is designed; second, the Latin-pelican optimization algorithm (LPOA) which fuses the population initialization of sine-Latin hypercube sampling, the prey position update of hierarchy system and the dynamic random search strategy is adopted, and the LPOA is used to optimize the lateral and longitudinal MPC controllers to realize the collaborative optimization of vehicle lateral and longitudinal trajectory tracking.

[0045] The present application proposes a vehicle lateral and longitudinal trajectory tracking collaborative optimization control method, and the specific steps are as follows: Step 1: establish a single-track model of the vehicle and an error calculation module, as follows: (1) (2) (3) (4) (5) (6) (7) Among them, formula (1) is the balance equation of the vehicle on the x axis according to Newton's second law; formula (2) is the balance equation of the vehicle on the y axis according to Newton's second law; formula (3) is the balance equation of the vehicle on the z axis according to Newton's second law; formula (4) is the established single-track model of the vehicle; formula (5) is the calculation formula of the lateral deviation of the vehicle, formula (6) is the calculation formula of the yaw angle deviation of the vehicle, and formula (7) is the calculation formula of the longitudinal speed error of the vehicle. In the formula, a , b respectively are the distances from the center of mass to the front and rear axes, m is the mass of the vehicle body, F xf ，F xr is the force of the front and rear wheels x in the direction ，F yf ，F yrthe front and rear wheels y the force in the direction of the front and rear wheels, C cf , C cr , C lf , C lr respectively the longitudinal stiffness and the cornering stiffness of the front and rear tires of the vehicle, I z the moment of inertia of the vehicle around the z axis of rotation, α the tire cornering angle, ξ the friction coefficient, F z the vertical load, s the slip ratio, C l the longitudinal stiffness of the tire, C c the cornering stiffness of the tire, the yaw angle, ξ r the angle between the road tangent and the earth coordinate system X axis, v r the longitudinal reference speed, e v the longitudinal speed error, v s the speed of the vehicle along the trajectory ξ f the front wheel steering angle, Y , X respectively the actual lateral and longitudinal positions of the vehicle, Y t , X t respectively the reference lateral and longitudinal positions of the vehicle Step 2: Design a lateral MPC controller, a longitudinal MPC upper controller and an inverse dynamics model-based lower controller based on the single-track model of the vehicle established in step 1; The lateral MPC controller is designed as follows: The state equation of the single-track model of the vehicle is , and the control variable is u = ξ f , and the state variable is After linearizing the single-track model of the vehicle, the following linear time-varying equation is obtained:

[0046] In the formula, is f to A(k)the Jacobian matrix of the system, B(k) t MPC modeling system t the state quantity at the moment, u t the system t the input quantity at the moment, is f the Jacobian matrix of the system, u After discretization processing, the state space expression is as follows:

[0047] In the formula, A(k)=I+TA(t) k the state matrix at the moment, (k)=TB(t) k+1 the state matrix at the next moment, η is the state transition matrix, η is the control input matrix, u k the input matrix, I is the sampling time, t+i , I is the unit matrix, B ρ The output quantity of the system is set as ε [ Y ] T After discretization processing, the output state space expression is as follows:

[0048] In the formula, ; The output quantity expression of the MPC modeling system is as follows:

[0049] In the formula, Y(k) represents the system output sequence in the prediction time domain, is the state prediction matrix, is the control increment prediction matrix, and △U(k) represents the control quantity increment in the prediction time domain.

[0050] According to the MPC modeling system model, the performance index (objective function) of the lateral MPC controller is constructed as follows:

[0051] In the formula, η t i t is the predicted t at the moment τ ​​​​​​​​System output at any moment, va is the weight coefficient, (k+i|k) is the relaxation factor, (k+i|k) ref is the system status reference value; The design of the longitudinal MPC upper controller is as follows: The longitudinal control is simplified to an integral and first-order inertial system, which can be expressed as:

[0052] Where, K is the system gain, (k+i|k) is the time constant, a is the acceleration, v For speed, a des is the reference acceleration; According to the above simplified integral and first-order inertial system, the continuous system state space equation of the vehicle longitudinal motion is expressed as:

[0053] In the formula, the state vector x =[ k+i ] T , u = a des System input for longitudinal MPC modeling, A 、 B The matrix is ​​as follows: ; The state space equation of the system is discretized to obtain the discrete system state equation as follows:

[0054] Where, k is the current sampling moment, T s is the sampling period, A k 、 B k The matrix is ​​as follows:

[0055] The vehicle speed v As the output quantity of the longitudinal MPC modeling system, the output equation is expressed as:

[0056] Where, x ( k ) The current state quantity, y ( k ) Output at the current moment,C = [1 0] ; The control objective of the longitudinal MPC modeling system is the speed tracking accuracy, and the performance index of the longitudinal MPC controller is defined as follows:

[0057] wherein, t -1 is the last sampling time, y p Δu(k+i) is the control output prediction value, y ref k+i is the control output reference value, Dim is the state at time t+1 predicted according to the sampling information at time t, k is the state at time t+1 predicted according to the sampling information at time t, UB is the control input increment at time t+1, LB , rand is the weight matrix; Q The design is based on an acceleration-driving force inverse dynamics model lower controller, specifically: R The lower controller is established based on an acceleration-driving force inverse dynamics model, including driving, braking control switching logic design and actuator mapping, the driving and braking control switching logic meets the following key elements: (1) driving and braking cannot be performed simultaneously; (2) driving and braking cannot be frequently switched, and the critical acceleration 0 is: a A vehicle speed and road adhesion coefficient correction factor is introduced, and the specific formula is as follows:

[0058] wherein, α0 is the driving force when the throttle opening is 0,

[0059] is the total resistance of the vehicle during driving, including the rolling resistance and air resistance of the vehicle, F v is the vehicle speed correction factor, used to suppress the nonlinear influence of air resistance at high speed, F μ is the road adhesion coefficient correction factor, which is increased at low adhesion coefficient to avoid slipping, and the buffer acceleration k = 0.05 m / s 2 is introduced to avoid frequent switching between driving and braking, specifically as follows: k q

[0060] ​​Step 3: Get the initial pelican algorithm;

[0061]

[0062] wherein, X i is the position of the i-th solution, ω is the dimensional space, δ j represents the upper bound of the i-th dimension, j δ j represents the lower bound of the i-th dimension, j N is the number of candidate solutions, ω is a random number between 0 and 1; The pelican begins to move towards the prey position, and the approaching prey stage position is updated as follows:

[0063]

[0064] wherein, is the position after the first stage update, is a random number with a value range of (0, 1), I is a random integer value in the range of [1, 2], P j is the position of the prey in the i-th dimension, j F p is the target fitness value of the prey; After the pelican reaches the water surface, it spreads its wings on the water surface, collects the prey in the throat bag, and sweeps across the water surface. The position update in this stage is as follows:

[0065]

[0066] wherein, β is a random number with a value range of [0, 1], R is a random integer value in the range of [1, 2], t is the current iteration number, T 1 is the maximum iteration number, is the position after the second stage update, is the target fitness value of the i-th pelican new position after the second stage update.

[0067] ​​​Step 4: The lateral error and the yaw error in the error calculation module are normalized and taken as the fitness function of the Step 3 pelican algorithm based on the vehicle monorail model established in Step 1. Considering the influence of the lateral error and the yaw error, the two error quantities are integrated into a comprehensive evaluation index through weighted combination, as follows:

[0068]

[0069]

[0070] wherein, is the vehicle yaw angle, is the reference yaw angle, , are the maximum and minimum values of the yaw error, respectively, is the yaw error, y e is the lateral error, y is the actual lateral position of the vehicle, y ref is the reference trajectory lateral position, y e_max , y e_min are the maximum and minimum values of the lateral error, respectively, w is the weighted adjustment coefficient.

[0071] The comprehensive evaluation index is brought into the following ITAE evaluation index as the objective function to guide the design or adjustment of the controller, and the specific mathematical expression is as follows:

[0072] Step 5: Based on the pelican optimization algorithm in Step 3, a Latin-pelican optimization algorithm (LPOA) is proposed, which integrates the population initialization based on sine-Latin hypercube sampling, the prey position update of the hierarchical system and the dynamic random search strategy.

[0073] Through population initialization based on sine-Latin hypercube sampling, the formula is as follows:

[0074] wherein, X i is the i th individual after population initialization, ls i is the corresponding LHS sequence, p is a random number with a value range of [0, 1], which is used to control the randomness of initialization; The prey position update of the hierarchy system is specifically:

[0075]

[0076]

[0077] wherein, D α , D β and D δ is Figure 3 the relative distance between the pelicans, α , β and Figure 4 the relative distance between the pelicans, X 1, X 2 and X 3 correspond to α , β and Figure 3 the pelican positions, X is Figure 4 the position of the pelican, C 1, C 2, C 3 is a random disturbance quantity, A 1, A 2, A 3 is a random variable, X F is the updated prey position.

[0078] The dynamic random search strategy is specifically: 1) Set the maximum number of iterations, the initial step size and the stopping criterion, initialize the current point, the current point function value, the optimal point and the optimal value; 2) Reset the counter and start a new round of search; 3) Randomly generate a disturbance vector within the current step size range; 4) Calculate the function value of the disturbance vector added to the current point; 5) If the function value is better than the optimal value, update the optimal point and the optimal value, and continue the next disturbance search; 6) If the function value is not better than the optimal value, but is still better than the current point function value, update the current point and the current function value, and continue the next disturbance search; 7) If neither of the above two conditions is met, calculate the function value of the disturbance vector in the opposite direction; 8) If the reverse disturbance function value is better than the optimal value, update the optimal point and the optimal value, and continue the next disturbance search; 9) If the reverse disturbance function value is not better than the optimal value, but the reverse disturbance function value is better than the current point function value, the current point and the current function value are updated, the counter is added by one, and the next disturbance search is continued; 10) When the counter reaches the maximum iteration number, the step size is reduced to half of the original, and the next round of search is started; 11) Determine whether the stopping criterion is met, if yes, end, otherwise repeat the above process.

[0079] Step 6: Using the fitness function obtained in step 4 and the Latin-Flamingo algorithm proposed in step 5, the lateral MPC controller and the longitudinal MPC upper controller designed in step 2 are optimized respectively, and the lower controller based on the acceleration-driving force inverse dynamics model is combined to realize the physical mapping of the control quantity, and finally the optimal weight matrix parameters are obtained, and the lateral and longitudinal combined trajectory tracking control of the vehicle is completed.

[0080] After the Flamingo population iteration is completed, the fitness sizes of the optimized lateral MPC controller and the longitudinal MPC upper controller are calculated, the optimal individual is selected, the position of the optimal individual is assigned to the weight matrix of the lateral MPC controller and the longitudinal MPC upper controller respectively, and then the optimized longitudinal MPC upper controller and the lower controller based on the acceleration-driving force inverse dynamics model are combined to realize the lateral and longitudinal combined trajectory tracking control of the vehicle.

[0081] Figure 4 is a comparison chart of the optimization ability of the Latin-Flamingo algorithm and the original Flamingo algorithm, Figure 4 is a lateral error and yaw angle error chart of vehicle trajectory tracking under different working conditions It can be seen from Figure 4 that the Latin-Flamingo algorithm designed in the application has better convergence speed, optimization ability, and better local optimal escape ability and exploration ability than the Flamingo algorithm.

[0082] ​ is a comparison chart of the trajectory tracking error before and after the vehicle lateral and longitudinal trajectory tracking collaborative optimization control method based on LPOA-MPC proposed in the application, ​ It can be seen from (a) and (b) that the MPC lateral and longitudinal combined trajectory tracking controller before and after LPOA optimization can realize effective tracking of the snake-shaped working condition under variable speed conditions. However, compared with the LPOA-MPC trajectory curve, the fitting degree of the reference trajectory is higher, especially in the transition stage of continuous curves, the lateral error amplitude is effectively suppressed within ±0.03 m, and there is no obvious lag or overshoot phenomenon. ​(c) and (d) can be seen that, compared with MPC controller, the oscillation amplitude of yaw angle error and yaw angular velocity error of LPOA-MPC is significantly reduced, which shows that the optimized controller can adapt to the change of road curvature faster, and significantly improves the stability of yaw dynamics on the premise of ensuring tracking accuracy. ​ (e) and (f) can be seen that, the transition of LPOA-MPC controller in uniform deceleration and uniform acceleration stage is smoother, especially when the speed is restored from 10 m / s to 15 m / s, the error convergence time of LPOA-MPC controller is significantly reduced, which shows that the optimized MPC controller quickly responds to control when the speed changes, in addition, the quantitative comparison of error interpolation shows that the maximum error of LPOA-MPC speed is reduced by 39.1%.

[0083] In summary, the LPOA-MPC controller shows stronger lateral control accuracy and longitudinal speed coordination ability when tracking the snake trajectory at variable speed. The LPOA-MPC controller has significant advantages in improving vehicle trajectory tracking accuracy, improving handling stability, and reducing lateral and longitudinal coupling interference.

[0084] Example two The vehicle lateral and longitudinal trajectory tracking cooperative optimization control system comprises: The first module is used for establishing a vehicle single-track model and an error calculation module; The second module is used for designing a lateral MPC controller, a longitudinal MPC upper controller and a lower controller based on an acceleration-driving force inverse dynamics model according to the vehicle single-track model; The third module is used for obtaining an initial pelican algorithm; The fourth module is used for normalizing the lateral error and the yaw angle error calculated by the error calculation module, and taking the normalization result as a fitness function of the pelican algorithm; The fifth module is used for improving the pelican algorithm to obtain a Latin-pelican optimization algorithm which integrates sine-Latin hypercube sampling population initialization, hierarchical prey position updating and dynamic random search strategy; The sixth module uses the fitness function and the Latin-pelican optimization algorithm to optimize the lateral MPC controller and the longitudinal MPC upper controller respectively, and combines the lower controller based on the acceleration-driving force inverse dynamics model to realize physical mapping of the control quantity, finally obtains the optimal weight matrix parameter, and completes the lateral and longitudinal joint trajectory tracking control of the vehicle.

[0085] It should be pointed out finally that the above embodiments are only used for illustrating the technical solutions of the present application but not for limiting the protection scope thereof, and although the present application has been described in detail with reference to the above embodiments, it should be understood by those skilled in the art that the specific embodiments of the present application can be changed, modified or replaced equivalently by those skilled in the art after reading the present application, but these changes, modifications or equivalent replacements are all within the protection scope of the claims of the present application to be approved.

Claims

1. A vehicle lateral and longitudinal trajectory tracking collaborative optimization control method, characterized in that: The following steps are involved: Step 1: Establish a vehicle single track model and error calculation module; Step 2: Based on the vehicle single track model established in step 1, design the lateral MPC controller, longitudinal MPC upper controller, and lower controller based on the acceleration-driving force inverse dynamics model. Step 3: Get the initial Pelican algorithm; Step 4: Normalize the lateral error and yaw angle error calculated by the error calculation module established in step 1, and use the normalized result as the fitness function of the Pelican algorithm in step 3; Step 5: Improve the Pelican algorithm in step 3 to obtain the Latin-Pelican optimization algorithm that integrates population initialization with sine-Latin hypercube sampling, prey position update based on the hierarchy, and dynamic random search strategy; Step 6: Use the fitness function obtained in step 4 and the Latin-Pelican algorithm proposed in step 5 to optimize the lateral MPC controller and longitudinal MPC upper-level controller designed in step 2, respectively. Combined with the lower-level controller based on the acceleration-driving force inverse dynamics model, the physical mapping of the control quantity is realized, and finally the optimal weight matrix parameters are obtained to complete the vehicle's lateral and longitudinal joint trajectory tracking control.

2. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 1, characterized in that: In step 1, the vehicle single track model and error calculation module are established as follows: （1） （2） （3） （4） （5） （6） （7） Among them, formula (1) is obtained based on Newton’s second law. x The equilibrium equation on the axis; Formula (2) is obtained based on Newton's second law. y The equilibrium equation on the axis; Formula (3) is obtained based on Newton's second law. z The equilibrium equation on the axis; Formula (4) is the established vehicle single track model; Formula (5) is the calculation formula for the vehicle lateral deviation, Formula (6) is the calculation formula for the vehicle yaw angle deviation, and Formula (7) is the calculation formula for the vehicle longitudinal velocity error, where, a , b are the distances from the center of mass to the front and rear axles, m For the body quality, F xf ，F xr For front and rear wheels x Directional force ，F yf ，F yr For front and rear wheels y Directional force, C cf , C cr , C lf , C lr are the longitudinal stiffness and lateral stiffness of the front and rear tires of the vehicle, I z For vehicles to go around z The moment of inertia of the shaft, α is the tire slip angle, μ is the friction coefficient, F z is the vertical load, s is the slip rate, C l is the tire longitudinal stiffness, C c is the tire cornering stiffness, is the yaw angle, φ r is the road tangent and the geodetic coordinate system X The angle between the axes, v r is the longitudinal reference speed, e v is the longitudinal velocity error, v s is the speed of the vehicle along the trajectory, δ f is the front wheel turning angle, Y , X are the actual horizontal and vertical positions of the vehicle, Y t , X t are the reference horizontal and vertical positions of the vehicle, respectively.

3. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 1, characterized in that: The design of the lateral MPC controller is as follows: The state equation of the vehicle single track model is: , the control variable is u = δ f , the state variable is , after linearizing the vehicle single track model, the following linear time-varying equation is obtained: Where, for f right ξ The Jacobian matrix of ξ ( t ) is the MPC modeling system t The state quantity at the moment, u ( t ) is the system t Time input, for f right u The Jacobian matrix of , after discretization, obtains the state space expression as follows: Where, ξ ( k ) is the current state matrix, ξ ( k+1 ) is the state matrix at the next moment, A(k) is the state transition matrix, B (k) is the control input matrix, u ( k ) is the input matrix, I is the sampling time, A(k)=I+TA(t) , I is the identity matrix, B(k)=TB (t) , set the output of the system to η =[ Y ] T , and discretize it to get the output state space expression as follows: Where, ; The output expression of the MPC modeling system is as follows: Where Y(k) represents the system output sequence in the prediction time domain, is the state prediction matrix, is the control increment prediction matrix, △U(k) represents the control increment in the prediction time domain Based on the above MPC modeling system, the performance indicators of the lateral MPC controller are as follows: Where, η ( t + i | t )for t Time-predicted t+i System output at any moment, ρ is the weight coefficient, ε is the relaxation factor, η ref is the system status reference value; The design of the longitudinal MPC upper controller is as follows: The longitudinal control is simplified to an integral and first-order inertial system, which can be expressed as: Where, K is the system gain, τ is the time constant, a is the acceleration, v For speed, a des is the reference acceleration; According to the above simplified integral and first-order inertial system, the continuous system state space equation of the vehicle longitudinal motion is expressed as: In the formula, the state vector x =[ va ] T , u = a des System input for longitudinal MPC modeling, A 、 B The matrix is ​​as follows: ; The state space equation of the system is discretized to obtain the discrete system state equation as follows: Where, k is the current sampling moment, T s is the sampling period, A k 、 B k The matrix is ​​as follows: The vehicle speed v As the output quantity of the longitudinal MPC modeling system, the output equation is expressed as: Where, x ( k ) The current state quantity, y ( k ) Output at the current moment, C =[1 0]; The control objective of the longitudinal MPC modeling system is speed tracking accuracy, and the performance indicators of the longitudinal MPC controller are defined as follows: Where, t -1 is the last sampling moment, y p (k+i|k) To control the output prediction value, y ref (k+i|k) To control the output reference value, (k+i|k) Based on k The sampling information at the moment is predicted k+i Momentary status, Δu(k+i) for k+i Control the input increment at all times, Q 、 R is the weight matrix; The design is based on the lower controller of the acceleration-driving force inverse dynamics model, specifically: The lower controller is established based on the acceleration-driving force inverse dynamics model, including the control switching logic design of driving and braking and the actuator mapping. The control switching logic of driving and braking meets the following key elements: (1) driving and braking cannot be performed at the same time; (2) driving and braking cannot be switched frequently, and the critical acceleration is defined. a 0 is: Introducing vehicle speed and road adhesion coefficient correction factors, the specific formula is as follows: Where, F α0 is the driving force when the throttle opening is 0, F It is the total resistance of the vehicle during driving, including the rolling resistance and air resistance of the vehicle. k v is the vehicle speed correction factor, which is used to suppress the nonlinear effect of air resistance under high-speed conditions. k μ It is the road adhesion coefficient correction factor, which increases when the adhesion coefficient is low to avoid slipping. On the basis of the above, the buffer acceleration is introduced. q =0.05m / s 2 To avoid frequent switching between driving and braking, please refer to the following: 。 4. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 1, characterized in that: The Pelican algorithm formula in step 3 is as follows: Where, X i is the position of the ith solution, Dim is a dimensional space, UB j Indicates the j The upper bound of dimension, LB j Indicates the j The lower bound of dimension, N is the number of candidate solutions, rand Is a random number between [0,1]; The pelican begins to move towards the prey's location. The position of the pelican during the approach phase is updated as follows: Where, is the updated position in the first phase, is a random number with a value range of (0,1). I is a random number, and its value range is a random integer value in [1,2]. P j For prey in j The location of the dimension, F p is the target fitness value of the prey; After the pelican reaches the water surface, it spreads its wings above the water surface and collects the prey in its throat pouch. The position update during the skimming phase is as follows: Where, β is a random number with a value range of [0,1]. R is a random number, and its value range is a random integer value in [1,2]. t is the current iteration number, T 1 is the maximum number of iterations, is the updated position in the second phase, is the target fitness value of the new position of the i-th pelican after the second phase update.

5. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 4, characterized in that: The fitness function in step 4 is specifically: Taking into account the influence of the two error quantities, lateral error and yaw angle error, the two error quantities are integrated into a comprehensive evaluation index through weighted combination, as follows: Where, is the vehicle yaw angle, is the reference yaw angle, 、 are the maximum and minimum values ​​of the yaw angle error, is the yaw angle error, y e is the lateral error, y is the actual lateral position of the vehicle, y ref is the lateral position of the reference trajectory, y e_max 、 y e_min are the maximum and minimum values ​​of the lateral error, w is the weighted adjustment factor. The comprehensive evaluation index is brought into the ITAE evaluation index as the objective function to guide the design or adjustment of the controller. The specific mathematical expression is as follows: 。 6. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 5, characterized in that: The population initialization of the sine-Latin hypercube sampling, the prey position update of the hierarchy, and the Latin-Pelican optimization algorithm of the dynamic random search strategy specifically include the following three mechanisms: Population initialization based on sine-Latin hypercube sampling; Hierarchical prey position updates; Dynamic random search strategy.

7. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 6, characterized in that: The population initialization based on sine-Latin hypercube sampling is as follows: Where: X i After the population is initialized i Individuals, ls i is the corresponding LHS sequence, p is a random number with a value range of [0,1], which is used to control the randomness of initialization; The prey position of the hierarchy is updated as follows: Where, D α 、 D β and D δ for ω Pelicans arrive α 、 β and δ The relative distance between pelicans, X 1. X 2 and X 3 correspond to α 、 β and δ Pelican Position, X for ω The position of the pelican, C 1, C 2, C 3 is the random perturbation amount, A 1, A 2, A 3 is a random variable, X F The updated prey location.

8. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 6, characterized in that: The dynamic random search strategy specifically includes: 1) Set the maximum number of iterations, initial step size and stopping criteria, initialize the current point, current point function value, optimal point and optimal value; 2) Reset the counter and start a new round of search; 3) Randomly generate a perturbation vector within the current step size range; 4) Calculate the function value after the perturbation vector is added to the current point; 5) If the function value is better than the optimal value, the optimal point and the optimal value are updated, the counter is incremented by one, and the next perturbation search is continued; 6) If the function value is not better than the optimal value, but still better than the current point function value, then update the current point and current function value, increment the counter by one, and continue the next perturbation search; 7) If neither of the above two conditions is satisfied, calculate the function value when the perturbation vector takes the opposite direction; 8) If the reverse perturbation function value is better than the optimal value, the optimal point and optimal value are updated, the counter is incremented by one, and the next perturbation search is continued; 9) If the reverse perturbation function value is not better than the optimal value, but the reverse perturbation function value is better than the current point function value, then update the current point and current function value, increment the counter by one, and continue the next perturbation search; 10) When the counter reaches the maximum number of iterations, the step size is reduced to half of the original one and the next round of search begins; 11) Determine whether the stopping criteria are met. If so, end the process; otherwise, repeat the above process.

9. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control method according to claim 6, characterized in that: Using the fitness function obtained in step 4 and the Latin-Pelican algorithm proposed in step 5, the lateral MPC controller and longitudinal MPC upper controller designed in step 2 are optimized respectively. Combined with the lower controller based on the acceleration-driving force inverse dynamics model, the physical mapping of the control quantity is realized, and the optimal weight matrix parameters are finally obtained to complete the vehicle's lateral and longitudinal joint trajectory tracking control. Specifically: After the pelican population iteration is completed, the optimized fitness of the lateral MPC controller and the longitudinal MPC super-controller are calculated respectively, the optimal individual is selected, and the position of the optimal individual is assigned to the weight matrix of the lateral MPC controller and the longitudinal MPC super-controller respectively. Then, the optimized longitudinal MPC super-controller is combined with the lower-level controller based on the acceleration-driving force inverse dynamics model to realize the vehicle's lateral and longitudinal trajectory tracking control.

10. The vehicle lateral and longitudinal trajectory tracking collaborative optimization control system is characterized by: include: The first module: used to establish a vehicle single track model and error calculation module; The second module is used to design the lateral MPC controller, the longitudinal MPC upper controller and the lower controller based on the acceleration-driving force inverse dynamics model according to the vehicle single track model; The third module: used to obtain the initial Pelican algorithm; The fourth module is used to normalize the lateral error and yaw angle error calculated by the error calculation module, and use the normalized results as the fitness function of the Pelican algorithm; The fifth module is used to improve the Pelican algorithm and obtain the Latin-Pelican optimization algorithm that integrates population initialization with sine-Latin hypercube sampling, hierarchical prey position update, and dynamic random search strategy; Module 6: Utilize the fitness function and Latin-Pelican algorithm to optimize the lateral MPC controller and longitudinal MPC upper-level controller respectively, and combine them with the lower-level controller based on the acceleration-driving force inverse dynamics model to realize the physical mapping of the control quantity, and finally obtain the optimal weight matrix parameters to complete the vehicle's lateral and longitudinal joint trajectory tracking control.

Images