Intelligent vehicle lateral and longitudinal cooperative control method considering time delay and external time-varying disturbance

Abstract

The application discloses a kind of intelligent car lateral and longitudinal collaborative control method considering time delay and external time-varying disturbance, comprising the following steps: obtaining target trajectory point at future time;Car-road state quantity is calculated;Discretization and iterative prediction are carried out;Set time domain constraint and carry out optimal solution;Determine final front wheel rotation angle and speed control instruction;Carry out lateral and longitudinal collaborative control.The application converts complex nonlinear intelligent car lateral and longitudinal control problem into a lateral and longitudinal collaborative model predictive controller for solving.The target trajectory decision module can obtain the information of target trajectory point at future time, prompting the lateral and longitudinal collaborative model predictive controller to calculate the control instruction considering total time delay time, to effectively improve the purpose of time delay problem.Then through preview feedforward and closed-loop feedback correction coordination mechanism can eliminate external time-varying disturbance, and the closed-loop speed feedback value and target speed superposition strategy can realize accurate longitudinal speed control.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle steering and speed control, and in particular to an intelligent vehicle lateral and longitudinal coordinated control method that takes into account time delay and external time-varying disturbances. Background Technology

[0002] Lateral path tracking methods are often designed based on a constant longitudinal speed, which lacks speed tracking control for the longitudinal components of the intelligent vehicle, thus lacking the ability for coordinated lateral and longitudinal control with controllable longitudinal speed. Furthermore, solving real-world coordinated lateral and longitudinal control problems requires considering external time-varying disturbances and time delays. External time-varying disturbances refer to real-time interference caused by the curvature of the external road, while time delays refer to the time lag generated when the control system signals are transmitted through the CAN bus. Failure to properly address time delays and external time-varying disturbances will result in untimely and inaccurate steering when the intelligent vehicle is cornering, potentially leading to deviation from the curve or even collisions with road boundaries, posing a risk to life. Therefore, developing a coordinated lateral and longitudinal control method for intelligent vehicles that considers time delays and external time-varying disturbances has become one of the key aspects of the intelligent vehicle field.

[0003] Traditional horizontal and vertical coordinated control methods typically employ algorithm combinations to address this problem, failing to simultaneously optimize path and velocity during controller development. Furthermore, they inadequately consider external time-varying disturbances and time delays, negatively impacting the performance of the coordinated control and potentially causing traffic accidents and threatening life and property. Summary of the Invention

[0004] To address the aforementioned problems in the existing technology, the present invention aims to provide a method for intelligent vehicle lateral and longitudinal coordinated control that considers time delay and external time-varying disturbances, which can effectively improve the accuracy and robustness of intelligent vehicle lateral and longitudinal coordinated control.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] A method for lateral and longitudinal coordinated control of intelligent vehicles that considers time delay and external time-varying disturbances is disclosed. The method utilizes an intelligent vehicle lateral and longitudinal coordinated control system, which includes an initialization module, a target trajectory decision module, and a lateral and longitudinal coordinated model predictive controller.

[0007] The initialization module is used to check whether the signal transmission and reception of the perception module, positioning module, planning module and chassis module are normal; load the parameters of the target trajectory decision module; load the parameters of the single-track intelligent vehicle dynamics model; load the parameters of the vehicle-road coupling optimization model with horizontal and vertical coordination; and load the parameters of the model predictive controller with horizontal and vertical coordination.

[0008] The target trajectory decision module calculates the road forward viewpoint based on the real-time planned path and planned speed. Based on the road forward viewpoint, it determines the target trajectory point on the reference trajectory at future times. The reference trajectory is issued by the planning module and includes reference position information, reference heading information, reference curvature information, and reference speed information. The target trajectory point information includes target position, target curvature, target heading, and target speed. This enables the lateral and longitudinal collaborative model predictive controller to calculate the control command in advance, taking into account the total time delay. The total time delay refers to the total time generated by the transmission of sensor signals and control signals through the intelligent vehicle controller local area network.

[0009] The vehicle-road coupling optimization model with lateral and longitudinal coordination is obtained by calculating the vehicle-road state variables from the single-track intelligent vehicle dynamics model and the target trajectory point and modeling them in the form of mathematical equations. The vehicle-road state variables refer to the lateral error, lateral error rate of change, heading error, heading error rate of change, and speed error generated between the current state of the intelligent vehicle and the target trajectory point during the movement process.

[0010] The lateral and longitudinal coordinated model prediction controller consists of a lateral aiming feedforward controller and a model prediction closed-loop feedback controller. The lateral aiming feedforward controller eliminates external time-varying disturbances caused by external road curvature based on the target curvature information obtained by the target trajectory decision module. The model prediction closed-loop feedback controller uses the vehicle-road state and the input control information to perform iterative calculations to obtain the lateral front wheel steering angle feedback value and the longitudinal speed feedback value.

[0011] The control method includes the following steps:

[0012] Step 1: Initialization

[0013] The initialization module checks whether the transmission and reception of information from the perception module, positioning module, planning module, and chassis module are normal. The perception module information, positioning module information, planning module information, and chassis module information respectively represent environmental obstacle information, intelligent vehicle pose information, reference trajectory, and intelligent vehicle speed. The intelligent vehicle pose information includes the intelligent vehicle's position and heading information. The reference trajectory includes reference position information, reference heading information, reference curvature information, and reference speed information. The initialization module loads the parameters of the target trajectory decision module, which include the number of forward-looking points and the sampling time.

[0014] The initialization module loads the parameters of the monorail intelligent vehicle dynamics model, the parameters of the lateral and longitudinal coordinated vehicle-road coupling optimization model, and the parameters of the lateral and longitudinal coordinated model predictive controller. The parameters of the monorail intelligent vehicle dynamics model and the parameters of the lateral and longitudinal coordinated vehicle-road coupling optimization model include the mass, moment of inertia, distance from the front and rear axles to the center of mass, and lateral stiffness of the front and rear wheels of the intelligent vehicle. The parameters of the lateral and longitudinal coordinated model predictive controller include the prediction time domain, the control time domain, the weight matrix of the output state variables of the model predictive controller in the prediction time domain, and the weight matrix of the input control variable increment.

[0015] The mathematical equations of the single-track intelligent vehicle dynamics model are as follows:

[0016]

[0017] In the formula: m is the mass of the intelligent vehicle; v y Lateral velocity; This is lateral acceleration; v x Longitudinal velocity; This refers to the yaw rate; I is the yaw acceleration; z l is the moment of inertia; f and l r These are the distances from the front and rear axles to the center of gravity, respectively; C αf and C αr These are the lateral stiffness of the front and rear wheels, respectively; δ f This refers to the steering angle of the front wheels.

[0018] The vehicle-road coupling optimization model for horizontal and vertical coordination is as follows:

[0019]

[0020] In the formula:

[0021]

[0022]

[0023] ξ(t) is the state variable matrix of the vehicle-road coupling optimization model with horizontal and vertical coordination. e is the first differential of ξ(t); d , and v e These refer to lateral error, rate of change of lateral error, heading error, rate of change of heading error, and speed error, respectively; A t Let B be the coefficient matrix of ξ(t). t C is the coefficient matrix of the control quantity matrix u(t). t For the desired yaw rate The coefficient matrix; δ f (t) represents the control input; v feed (t) represents the speed feedback control quantity in the model-predicted closed-loop feedback controller; The external time-varying disturbance is y(t); y(t) is the state output matrix; I 5×5 It is a 5×5 identity matrix.

[0024] Step 2: Obtain the target trajectory points at future moments.

[0025] The reference trajectory is input into the target trajectory decision module. Taking into account the influence of the total time delay, the output information is the target position, target curvature, target heading, and target speed information of the target trajectory point at future time.

[0026] The mathematical equation for the target trajectory decision module is as follows:

[0027]

[0028] In the formula: j is the index value corresponding to the point on the reference trajectory in each frame, i is the intermediate variable of the index value j, and v i This represents the reference speed information corresponding to the index value i; x and y are the horizontal and vertical coordinates of the current point of the intelligent vehicle in the world coordinate system. The heading of the intelligent vehicle in the world coordinate system; the forward-looking point G, located at a path length of ΔH from the current point of the intelligent vehicle, has x and y coordinates respectively. a and y a Where n is the number of forward-looking points, and the total time delay is t. d =nT; Point P is the target trajectory point, and the x and y coordinates of point P are respectively x, y, and y. p and Y p Point P is the point on the reference trajectory mapped from the road's forward-looking point G; T is the sampling time.

[0029] Step 3: Calculate vehicle-road state variables

[0030] The lateral error, lateral error rate of change, heading error, heading error rate of change, and speed error generated between the current state of the intelligent vehicle and the target trajectory point during the intelligent vehicle's motion are calculated. Based on the single-track intelligent vehicle dynamics model established in step 1, a vehicle-road coupling optimization model with lateral and longitudinal coordination is obtained. The vehicle-road state variables include lateral error, lateral error rate of change, heading error, heading error rate of change, and speed error, and their calculation formulas are as follows:

[0031]

[0032] In the formula: X p and Y p The x and y coordinates of the target trajectory point in the world coordinate system; For the target course; The differential form of the target heading; The differential form of the heading of an intelligent vehicle in a world coordinate system; v p Target speed; ρ represents the velocity of the intelligent vehicle's projection point in the Frenet coordinate system, which refers to a coordinate system established around the lane center; p The target curvature.

[0033] Step 4: Perform discretization and iterative prediction

[0034] Discretizing the vehicle-road coupling optimization model established in step 1 yields the ideal real-time model, whose mathematical equations are as follows:

[0035]

[0036] In the formula: ξ(k) is the state quantity matrix of the discretized vehicle-road coupling optimization model with lateral and longitudinal coordination, and ξ(k+1) is the state quantity matrix of the discretized vehicle-road coupling optimization model with lateral and longitudinal coordination at time k+1. The state quantity matrix includes the lateral error, lateral error rate of change, heading error, heading error rate of change, and speed error corresponding to each sampling time. B d =B t T; E d =I 5×5 A d Let B be the coefficient matrix of the discretized ξ(k). d C is the coefficient matrix of the discrete control quantity matrix u(k). d The external time-varying disturbance described after discretization; δ f (k) represents the discrete control input; v feed (k) represents the speed feedback control quantity in the discrete model predictive closed-loop feedback controller; y(k) represents the discrete state output matrix; I 5×5 It is a 5×5 identity matrix.

[0037] The discrete form of the vehicle-road coupling optimization model considering time delay and lateral coordination is a time-delay system, and its mathematical equation is:

[0038]

[0039] Where: δ f (kt d ) represents the control input for the time-delay system; v feed (kt d ) represents the speed feedback control quantity of the time-delay system; the total time delay is t. dThe resulting impact is mitigated by the target trajectory decision module designed in step 2. This module transforms the time-delay system into an ideal real-time model.

[0040] Then, based on the ideal real-time model, the prediction time domain N is set. p and control time domain N c The iterative prediction model of the horizontally and vertically coordinated model predictive controller in the prediction time domain is determined by an iterative prediction method, and is expressed in the form of a state-space equation as follows:

[0041]

[0042] In the formula: Ξ(k), Z(k) and ΔU are the output state variables, vehicle-road state variables to be optimized and control input increment sequences of the model predictive controller with horizontal and vertical coordination in the prediction time domain, respectively. for The coefficient matrix; Let be the coefficient matrix of ΔU; Ψ represents the external time-varying disturbance of the model predictive controller in the prediction time domain; Ψ represents the output equation of the horizontally and vertically coordinated model predictive controller in the prediction time domain. Θ is the coefficient matrix of the model predictive controller; Θ is the coefficient matrix of ΔU in the output equation of the horizontal and vertical collaborative model predictive controller in the prediction time domain; Φ is the external time-varying disturbance in the output equation of the horizontal and vertical collaborative model predictive controller in the prediction time domain.

[0043]

[0044] Step 5: Set time-domain constraints and perform optimization solutions.

[0045] Based on the horizontal and vertical collaborative model predictive controller determined in step 4, the iterative prediction model in the prediction time domain is transformed into a standard quadratic programming problem with multiple safety time domain constraints, and its mathematical equation form is:

[0046]

[0047] In the formula: J(k) is the objective function of the model predictive controller for horizontal and vertical coordination; u min (k+j) is the minimum value of the time-domain constraint of u(k+j), u max (k+j) is the maximum value of the time-domain constraint of u(k+j); Δu min (k+j) is the minimum value of the time-domain constraint of Δu(k+j), Δu max (k+j) is the maximum value of the time-domain constraint of Δu(k+j); η min (k+i) represents the minimum time-domain constraint value of the output state variable, ηmax (k+i) represents the maximum time-domain constraint value of the output state variable. and These are the weight matrices for the output state variables and the input control variable increments of the model predictive controller in the prediction time domain, respectively. G and H are both coefficient matrices related to ΔU in the quadratic programming solution process.

[0048] Step 6: Determine the final front wheel steering angle and speed control command

[0049] The lateral and longitudinal coordinated model predictive controller consists of a lateral aiming feedforward controller and a model predictive closed-loop feedback controller. The final front wheel steering angle command is obtained by superimposing the front wheel steering angle value calculated by the lateral aiming feedforward controller and the lateral front wheel steering angle feedback value calculated by the model predictive closed-loop feedback controller. The final speed control command is obtained by superimposing the target speed information and the longitudinal speed feedback value calculated by the model predictive closed-loop feedback controller. The mathematical equation expression of the lateral aiming feedforward controller is:

[0050]

[0051] In the formula: Δρ max Δρ is the maximum value of the target curvature increment constraint. min The minimum value of the target curvature increment constraint; L is the wheelbase of the intelligent vehicle and satisfies L = l f +l r ;ρ p (k) represents the target curvature acquired at time k, ρ p (k+1) represents the target curvature acquired at time k+1, and Δρ is the difference between the target curvature acquired at time k+1 and time k; δ ff The front wheel steering angle value calculated for the lateral aiming feedforward controller.

[0052] The calculation formulas for the final front wheel steering angle command and the final speed control command are as follows:

[0053] δ f =δ mpc +δ ff

[0054] v cmd =v p +v feed

[0055] Where: δ mpc The lateral front wheel steering angle feedback value calculated by the model prediction closed-loop feedback controller; v feed To predict the velocity feedback control quantity in the closed-loop feedback controller for the model.

[0056] Step 7: Perform horizontal and vertical coordinated control

[0057] The final front wheel steering angle command and final speed control command determined in step 6 are filtered and sent to the intelligent vehicle as control signals at a fixed frequency. The intelligent vehicle performs lateral and longitudinal coordinated control based on the final front wheel steering angle command and final speed control command. Further, it is determined whether the intelligent vehicle has reached its destination. If so, the control task is completed; otherwise, proceed to step 1.

[0058] The present invention has the following beneficial effects:

[0059] 1. This invention transforms the complex nonlinear lateral and longitudinal control problem of intelligent vehicles into a solution using a multi-input multi-output (MIMO) lateral and longitudinal collaborative model predictive controller. A target trajectory decision module is designed to acquire information about the target trajectory points at future times, enabling the lateral and longitudinal collaborative model predictive controller to calculate control commands considering the total time delay in advance, thus effectively improving the time delay problem. Then, through a pre-aiming feedforward and closed-loop feedback correction coordination mechanism, external time-varying disturbances can be eliminated. The strategy of superimposing the closed-loop speed feedback value with the target speed achieves precise longitudinal speed control.

[0060] 2. This invention takes into account external time-varying disturbances, time-delay disturbances, and multiple time-domain safety constraints in the lateral and longitudinal coordinated control of trajectory tracking in reality. It can effectively solve the impact of external time-varying disturbances and time-delay disturbances on the accuracy and stability of lateral and longitudinal coordinated control. It can achieve satisfactory performance in lane centering, steering response and ride comfort, tracking accuracy and adaptive control of cornering speed, while ensuring the stability of control. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the present invention.

[0062] Figure 2 This is a schematic diagram of the intelligent vehicle lateral and longitudinal coordinated control system of the present invention.

[0063] Figure 3 This is a schematic diagram of the vehicle-road coupling optimization model of horizontal and vertical coordination of the present invention.

[0064] Figure 4 This is a flowchart of an embodiment of the present invention.

[0065] Figure 5 This is a schematic diagram of an existing intelligent vehicle that fails to turn in time when cornering and collides with the road boundary. Detailed Implementation

[0066] The following is for reference only. Figure 1-5 The present invention will be further described below.

[0067] like Figure 1The diagram illustrates the principle of a lateral and longitudinal coordinated control method for intelligent vehicles, considering time delays and external time-varying disturbances. This method utilizes a lateral and longitudinal coordinated model predictive controller to achieve precise control of the lateral front wheel steering angle and longitudinal speed of the intelligent vehicle. The lateral and longitudinal coordinated model predictive controller consists of a lateral preview feedforward controller and a model predictive closed-loop feedback controller. By designing a target trajectory decision module to obtain target trajectory point information at future times, the lateral and longitudinal coordinated model predictive controller can calculate control commands considering the total time delay in advance, effectively improving the time delay problem. Then, an iterative prediction model for the lateral and longitudinal coordinated model predictive controller in the prediction time domain is determined through iterative prediction. This iterative prediction model is further transformed into a standard quadratic programming problem with multiple safety time-domain constraints and solved.

[0068] The final front wheel steering angle command is obtained by superimposing the front wheel steering angle value calculated by the lateral anti-planning feedforward controller and the lateral front wheel steering angle feedback value calculated by the model prediction closed-loop feedback controller. The lateral anti-planning feedforward controller is used to eliminate external time-varying disturbances caused by the curvature of the external road. The final speed control command is obtained by superimposing the target speed information and the longitudinal speed feedback value calculated by the model prediction closed-loop feedback controller. If the time delay and external time-varying disturbance problems mentioned in the lateral and longitudinal coordinated control of intelligent vehicles cannot be well resolved, it will cause the intelligent vehicle to be slow to turn and its accuracy to decrease when cornering, even deviating from the curve, or even colliding with the road boundary, bringing life safety risks, such as... Figure 5 As shown.

[0069] like Figure 2 As shown, this invention provides an intelligent vehicle lateral and longitudinal cooperative control system, which includes control modules such as an initialization module, a target trajectory decision module, and a lateral and longitudinal cooperative model prediction controller.

[0070] according to Figure 1 and Figure 2 This invention provides a method for intelligent vehicle lateral and longitudinal coordinated control that considers time delay and external time-varying disturbances, utilizing an intelligent vehicle lateral and longitudinal coordinated control system for control. The specific design steps of the intelligent vehicle lateral and longitudinal coordinated control system are as follows:

[0071] Step 1: Design the target trajectory decision module

[0072] The reference trajectory is input into the target trajectory decision module. Taking into account the influence of the total time delay, the output information is the target position, target curvature, target heading, and target speed information of the target trajectory point at future time.

[0073] The mathematical equation for the target trajectory decision module is as follows:

[0074]

[0075] In the formula: j is the index value corresponding to the point on the reference trajectory in each frame, i is the intermediate variable of the index value j, and v i This represents the reference speed information corresponding to the index value i; x and y are the horizontal and vertical coordinates of the current point of the intelligent vehicle in the world coordinate system. The heading of the intelligent vehicle in the world coordinate system; the forward-looking point G, located at a path length of ΔH from the current point of the intelligent vehicle, has x and y coordinates respectively. a and y a Where n is the number of forward-looking points, and the total time delay is t. d =nT; Point P is the target trajectory point, and the x and y coordinates of point P are respectively x, y, and y. p and Y p Point P is the point on the reference trajectory mapped from the road's forward-looking point G; T is the sampling time.

[0076] Step 2: Design a vehicle-road coupling optimization model that coordinates horizontal and vertical directions.

[0077] The vehicle-road coupling optimization model, which integrates horizontal and vertical coordination, is derived from the dynamics model of a single-track intelligent vehicle and the vehicle-road state variables calculated from the target trajectory points obtained in step 1, and modeled in the form of mathematical equations, such as... Figure 3 As shown, the model mainly includes target trajectory point information and a single-track intelligent vehicle dynamics model, constructing the path tracking control relationship between the target trajectory points and the single-track dynamics model. It also includes intelligent vehicle attitude information and the Frenet coordinate system n. r o r τ r The coordinate systems include the geodetic coordinate system XOY and the intelligent vehicle coordinate system xoy. The vehicle-road state variables refer to the lateral error, lateral error rate of change, heading error, heading error rate of change, and speed error generated between the current state and the target trajectory point during the intelligent vehicle's motion.

[0078] The dynamic model of the single-track intelligent vehicle is established in the form of mathematical equations as follows:

[0079]

[0080] In the formula: m is the mass of the intelligent vehicle; v y Lateral speed; This is lateral acceleration; v x This refers to the longitudinal speed of the vehicle. This refers to the yaw rate; I is the yaw acceleration; z l is the moment of inertia; f and l r These are the distances from the front and rear axles to the center of gravity, respectively; C αf and C αrThese are the lateral stiffness of the front and rear wheels, respectively; δ f This refers to the steering angle of the front wheels.

[0081] Furthermore, the vehicle-road coupling optimization model for horizontal and vertical coordination is established in the form of mathematical equations as follows:

[0082]

[0083] In the formula:

[0084]

[0085] ξ(t) is the state variable matrix of the vehicle-road coupling optimization model with horizontal and vertical coordination. e is the first differential of ξ(t); d , and v e These refer respectively to the lateral error, the rate of change of lateral error, the heading error, the rate of change of heading error, and the speed error; A t Let B be the coefficient matrix of ξ(t). t C is the coefficient matrix of the control quantity matrix u(t). t For the desired yaw rate The coefficient matrix; δ f (t) represents the control input; v feed (t) represents the speed feedback control quantity in the model-predicted closed-loop feedback controller; The external time-varying disturbance is y(t); y(t) is the state output matrix; I 5×5 It is a 5×5 identity matrix.

[0086] Based on the target trajectory point information obtained in step 1, the calculation formulas for the lateral error, lateral error rate of change, heading error, heading error rate of change, and velocity error are as follows:

[0087]

[0088] In the formula: X p and Y p These are the position coordinates of the target trajectory point in the world coordinate system. For the target course; The differential form of the target heading; The differential form of the heading of an intelligent vehicle in a world coordinate system; v p Target speed; ρ represents the velocity of the intelligent vehicle's projection point in the Frenet coordinate system, which refers to a coordinate system established around the lane center; p The target curvature.

[0089] Step 3: Design an iterative prediction model

[0090] Discretizing the vehicle-road coupling optimization model designed in step 2 yields the ideal real-time model, whose mathematical equations are as follows:

[0091]

[0092] In the formula: ξ(k) is the state quantity matrix of the discretized vehicle-road coupling optimization model with lateral and longitudinal coordination, and ξ(k+1) is the state quantity matrix of the discretized vehicle-road coupling optimization model with lateral and longitudinal coordination at time (k+1), which specifically includes the lateral error, lateral error rate of change, heading error, heading error rate of change and speed error corresponding to each sampling time. B d =B t T; E d =I 5×5 A d Let B be the coefficient matrix of the discretized ξ(k). d C is the coefficient matrix of the discrete control quantity matrix u(k). d The external time-varying disturbance described after discretization; δ f (k) represents the discrete control input; v feed (k) represents the speed feedback control quantity in the discrete model predictive closed-loop feedback controller; y(k) represents the discrete state output matrix; I 5×5 It is a 5×5 identity matrix.

[0093] The discrete form of the vehicle-road coupling optimization model considering time delay and lateral coordination is a time-delay system, and its mathematical equation is:

[0094]

[0095] Where: δ f (kt d ) represents the control input for the time-delay system; v feed (kt d ) represents the speed feedback control quantity of the time-delay system; the total time delay is t. d The resulting impact is mitigated by the target trajectory decision module designed in step 2. This module transforms the time-delay system into an ideal real-time model.

[0096] Furthermore, the current state variable ξ(k) and the control variable u(k-1) from the previous time step are combined to reconstruct a new state variable.

[0097]

[0098] Further analysis revealed the following:

[0099]

[0100] The iterative prediction basic model with control increment is obtained, and its state-space equation is as follows:

[0101]

[0102] In the formula: Let k be the state variable matrix of the iterative prediction base model at time k. The state variable matrix of the basic model for iterative prediction at time k+1; for The coefficient matrix; Let be the coefficient matrix of Δu(k); η(k) represents the external time-varying disturbance in the iterative prediction base model; η(k) represents the output state in the iterative prediction base model. In the output equation of the iterative prediction base model The coefficient matrix.

[0103] Then, based on the ideal real-time model, the prediction time domain N is set. p Control time domain N c Based on the aforementioned iterative prediction model, the iterative prediction model of the horizontally and vertically coordinated model prediction controller in the prediction time domain is determined, and expressed in the form of state-space equations as follows:

[0104]

[0105] In the formula: Ξ(k), Z(k) and ΔU are the output state variables, vehicle-road state variables to be optimized and control input increment sequences of the model predictive controller with horizontal and vertical coordination in the prediction time domain, respectively. for The coefficient matrix; Let be the coefficient matrix of ΔU; Ψ represents the external time-varying disturbance of the model predictive controller in the prediction time domain; Ψ represents the output equation of the model predictive controller in the prediction time domain. Θ is the coefficient matrix of the model predictive controller in the prediction time domain; Φ is the external time-varying disturbance in the output equation of the model predictive controller in the prediction time domain.

[0106]

[0107]

[0108] Step 4: Design the quadratic programming problem

[0109] Based on the iterative prediction model designed in step 3, the objective function of the horizontal and vertical collaborative model prediction controller is further designed as follows:

[0110]

[0111] In the formula: and These are the weight matrices for the increments of the output state variables and the input control variables, respectively.

[0112] Furthermore, based on the objective function, a standard quadratic programming problem with multiple security time-domain constraints can be designed, and its mathematical equation form is:

[0113]

[0114] In the formula: J(k) is the objective function of the model predictive controller for horizontal and vertical coordination; u min (k+j) is the minimum value of the time-domain constraint of u(k+j), u max (k+j) is the maximum value of the time-domain constraint of u(k+j); Δu min (k+j) is the minimum value of the time-domain constraint of Δu(k+j), Δu max (k+j) is the maximum value of the time-domain constraint of Δu(k+j); η min (k+i) represents the minimum time-domain constraint value of the output state variable, η max (k+i) represents the maximum time-domain constraint value of the output state variable. and These are the weight matrices for the output state variables and the input control variable increments of the model predictive controller in the prediction time domain, respectively. G and H are both coefficient matrices related to ΔU in the quadratic programming solution process.

[0115] The lateral front wheel steering angle feedback value δ of the model predictive closed-loop feedback controller is obtained by solving the standard quadratic programming problem described above. mpc and longitudinal velocity feedback value v feed .

[0116] Step 5: Design the lateral aiming feedforward controller

[0117] The design of the lateral aiming feedforward controller, based on the target curvature information obtained in step 1, is expressed by the following mathematical equation:

[0118]

[0119] In the formula: Δρ max Δρ is the maximum value of the target curvature increment constraint. min The minimum value of the target curvature increment constraint; L is the wheelbase of the intelligent vehicle and satisfies L = l f +l r ;ρp (k) represents the target curvature acquired at time k, ρ p (k+1) represents the target curvature acquired at time k+1, and Δρ is the difference between the target curvature acquired at time k+1 and time k; δ ff The front wheel steering angle value calculated for the lateral aiming feedforward controller.

[0120] Step 6: Based on Steps 4 and 5, design the final front wheel steering angle and speed control laws as follows:

[0121] δ f =δ mpc +δ ff

[0122] v cmd =v p +v feed

[0123] Step 7: Filter the final front wheel steering angle command and final speed control command determined in Step 6, and send them as control signals to the intelligent vehicle at a fixed frequency. The intelligent vehicle performs lateral and longitudinal coordinated control based on the final front wheel steering angle command and final speed control command. Further, determine whether the intelligent vehicle has reached its destination. If so, the control task is completed; otherwise, repeat steps 1 to 7 to ensure the intelligent vehicle safely and stably achieves lateral and longitudinal coordinated control.

[0124] This invention is not limited to the details of the above embodiments. Any equivalent concept or modification within the technical scope disclosed in this invention shall be included within the protection scope of this invention.

Claims

1.A method for intelligent vehicle lateral-longitudinal cooperative control considering time delay and external time-varying disturbance, which is controlled by an intelligent vehicle lateral-longitudinal cooperative control system, the system comprising an initialization module, a target trajectory decision module and a lateral-longitudinal cooperative model predictive controller; The initialization module is used to check whether the signals of the perception module, the positioning module, the planning module and the chassis module are normal, load the parameters of the target trajectory decision module, load the parameters of the single-track intelligent vehicle dynamics model, load the parameters of the lateral-longitudinal cooperative vehicle-road coupling optimization model, and load the parameters of the lateral-longitudinal cooperative model predictive controller; The target trajectory decision module calculates a road front view point according to a real-time planning path and a planning speed, and determines a target trajectory point on a reference trajectory at a future time in advance according to the road front view point, the reference trajectory being issued by a planning module, and the reference trajectory including reference position information, reference heading information, reference curvature information, and reference speed information. The information of the target trajectory point includes target position, target curvature, target heading and target speed, which prompts the lateral-longitudinal cooperative model predictive controller to calculate the control instruction considering the total time delay in advance, the total time delay referring to the total time of sensor signals and control signals transmitted through the intelligent vehicle controller local area network; The lateral-longitudinal cooperative vehicle-road coupling optimization model is obtained by calculating the vehicle-road state quantity between the current state and the target trajectory point in the intelligent vehicle motion process and modeling in the form of mathematical equations, the vehicle-road state quantity referring to the lateral error, the lateral error rate, the heading error, the heading error rate and the speed error; The lateral-longitudinal cooperative model predictive controller is composed of a lateral preview feedforward controller and a model predictive closed-loop feedback controller, the lateral preview feedforward controller based on the target curvature information obtained by the target trajectory decision module to eliminate the external time-varying disturbance caused by the external road curvature, and the model predictive closed-loop feedback controller using the vehicle-road state and the input control information to obtain the lateral front wheel steering angle feedback value and the longitudinal speed feedback value through iterative calculation; The control method comprises the following steps: Step 1: initialization The initialization module checks whether the signals of the perception module, the positioning module, the planning module and the chassis module are normal, the signals of the perception module, the positioning module, the planning module and the chassis module referring to the environmental obstacle information, the intelligent vehicle pose information, the reference trajectory and the intelligent vehicle speed respectively, the intelligent vehicle pose information including the position and heading information of the intelligent vehicle, and the reference trajectory including the reference position information, the reference heading information, the reference curvature information and the reference speed information; The initialization module loads the parameters of the target trajectory decision module, the parameters of the target trajectory decision module including the number of preview points and the sampling time. The initialization module loads the monorail intelligent vehicle dynamics model parameters, loads the lateral-longitudinal collaborative vehicle-road coupling optimization model parameters, and loads the lateral-longitudinal collaborative model predictive controller parameters; the monorail intelligent vehicle dynamics model parameters and the lateral-longitudinal collaborative vehicle-road coupling optimization model parameters both include the mass, the moment of inertia, the distance from the front and rear axles to the center of mass, and the side stiffness of the front and rear wheels of the intelligent vehicle; the lateral-longitudinal collaborative model predictive controller parameters include the model predictive controller prediction horizon, the control horizon, the output state quantity weight matrix of the model predictive controller in the prediction horizon, and the weight matrix of the input control quantity increment; The mathematical equation of the monorail intelligent vehicle dynamics model is as follows: where m is the mass of the intelligent vehicle; v y is the lateral velocity; is the lateral acceleration; v x is the longitudinal velocity; is the yaw angular velocity; is the yaw angular acceleration; I z is the moment of inertia; l f and l r are the distances from the front and rear axles to the center of mass, respectively; C αf and C αr are the lateral stiffness of the front and rear wheels, respectively; δ f is the front wheel steering angle; The lateral-longitudinal collaborative vehicle-road coupling optimization model is as follows: In the formula: ξ(t) is the state matrix of the lateral-longitudinal cooperative vehicle-road coupled optimization model is the first order differential of ξ(t); e d , and v e denote lateral error, lateral error rate, heading error, heading error rate and speed error, respectively; A t is the coefficient matrix of ξ(t), B t is the coefficient matrix of the control matrix u(t), C t is the coefficient matrix of the expected yaw rate ; δ f (t) is the control input; v feed (t) is the speed feedback control quantity in the model predictive closed-loop feedback controller; is the external time-varying disturbance; y(t) is the state output matrix; I 5×5 is a 5x5 unit matrix; Step 2: Obtain the target trajectory point at the future time The reference trajectory is input into the target trajectory decision module, the influence of the total time delay is considered, and the output information is the target position, the target curvature, the target heading, and the target speed information of the target trajectory point at the future time; The mathematical equation of the target trajectory decision module is as follows: wherein: j is an index value corresponding to a point on the reference trajectory of each frame, i is an intermediate variable of the index value j, v i is the reference speed information corresponding to the index value i; x and y are the horizontal and vertical coordinates of the current point of the intelligent vehicle in the world coordinate system; is the heading of the intelligent vehicle in the world coordinate system; G is a road preview point at a preview path length of ΔH from the current point of the intelligent vehicle, the horizontal and vertical coordinates of the point G are x a and y a , n is the number of preview points, the total time lag time is t d =nT; P is a target trajectory point, the horizontal and vertical coordinates of the point P are X p and Y p , the point P is a point on the reference trajectory mapped by the road preview point G; T is a sampling time; Step 3: Calculate the vehicle-road state quantity The lateral error, the lateral error change rate, the heading error, the heading error change rate, and the speed error generated between the current state of the intelligent vehicle in the motion process and the target trajectory point are calculated, the lateral-longitudinal collaborative vehicle-road coupling optimization model is obtained based on the monorail intelligent vehicle dynamics model established in step 1; the vehicle-road state quantity includes the lateral error, the lateral error change rate, the heading error, the heading error change rate, and the speed error, and the calculation formula is as follows: wherein: X p and Y p are the horizontal and vertical coordinates of the target trajectory point in the world coordinate system; is the target heading; is the differential form of the target heading; is the differential form of the heading of the intelligent vehicle in the world coordinate system;v p is the target speed; is the projected point speed of the intelligent vehicle in the Frenet coordinate system, which refers to the coordinate system established with the lane center; p p is the target curvature; Step 4: Discretization and iterative prediction The lateral-longitudinal collaborative vehicle-road coupling optimization model established in step 1 is discretized, that is, the ideal real-time model, and the mathematical equation form is as follows: In the formula, ξ(k) is a state quantity matrix of a discretized vehicle-road coupling optimization model of lateral and longitudinal coordination, ξ(k+1) is a state quantity matrix of the discretized vehicle-road coupling optimization model of lateral and longitudinal coordination at k+1 time, the state quantity matrix includes a lateral error, a lateral error change rate, a heading error, a heading error change rate and a speed error corresponding to each sampling time; B d = B t T; E d = I 5×5 ; A d is a coefficient matrix of ξ(k) after discretization, B d is a coefficient matrix of a control quantity matrix u(k) after discretization, C d is the external time-varying disturbance after discretization; δ f (k) is a control input after discretization; v feed (k) is a speed feedback control quantity in a model predictive closed-loop feedback controller after discretization; y(k) is a state output matrix after discretization; I 5×5 is a 5*5 unit matrix; The discretized form of the lateral-longitudinal collaborative vehicle-road coupling optimization model considering the time delay is a time delay system, and the mathematical equation is as follows: wherein: δ f (k-t d ) is the control input of the time-delay system; v feed (k-t d ) is the speed feedback control quantity of the time-delay system; the total time delay time t d The influence caused by the total time delay time t is improved by a target trajectory decision module designed in step 2; the time-delay system is converted into an ideal real-time model through the designed target trajectory decision module; Then, based on the ideal real-time model, set the prediction horizon N p and control horizon N c The iterative prediction model of the horizontal and vertical collaborative model predictive controller in the prediction horizon is determined by the iterative prediction method, which is expressed in the form of state space equation: In the formula: Ξ(k), Z(k) and ΔU are the output state variables, vehicle-road state variables to be optimized and control input increment sequences of the model predictive controller with horizontal and vertical coordination in the prediction time domain, respectively. for The coefficient matrix; Let be the coefficient matrix of ΔU; Ψ represents the external time-varying disturbance of the model predictive controller in the prediction time domain; Ψ represents the output equation of the horizontally and vertically coordinated model predictive controller in the prediction time domain. Θ is the coefficient matrix of the model predictive controller with horizontal and vertical coordination in the prediction time domain; Φ is the external time-varying disturbance in the output equation of the model predictive controller with horizontal and vertical coordination in the prediction time domain. Step 5: Set the time domain constraint and solve the optimization According to the iterative prediction model of the lateral-longitudinal collaborative model predictive controller in the prediction horizon determined in step 4, a standard quadratic programming problem containing multiple safety time domain constraints is converted, and the mathematical equation form is as follows: wherein: J(k) is the objective function of the model predictive controller for lateral-longitudinal coordination; u min (k+j) is the minimum value of the time domain constraint of u(k+j), u max (k+j) is the maximum value of the time domain constraint of u(k+j); Δu min (k+j) is the minimum value of the time domain constraint of Δu(k+j), Δu max (k+j) is the maximum value of the time domain constraint of Δu(k+j); η min (k+i) is the minimum value of the time domain constraint of the output state quantity, η max (k+i) is the maximum value of the time domain constraint of the output state quantity; and are the weight matrix of the output state quantity and the weight matrix of the input control quantity increment of the model predictive controller in the prediction time domain, respectively; G and H are the coefficient matrices related to ΔU in the process of solving the quadratic programming. Step 6: Determine the final front wheel steering angle and speed control instruction The lateral-longitudinal collaborative model predictive controller is composed of a lateral preview feedforward controller and a model predictive closed-loop feedback controller, the final front wheel steering angle instruction is obtained by superimposing the front wheel steering angle value calculated by the lateral preview feedforward controller and the lateral front wheel steering angle feedback value calculated by the model predictive closed-loop feedback controller, and the final speed control instruction is obtained by superimposing the target speed information and the longitudinal speed feedback value calculated by the model predictive closed-loop feedback controller; the mathematical equation expression of the lateral preview feedforward controller is as follows: wherein: Δρ max is a target curvature increment constraint maximum value, Δρ min is a target curvature increment constraint minimum value; L is the wheelbase of the intelligent vehicle and satisfies L = l f + l r ; ρ p (k) is a target curvature obtained at time k, ρ p (k+1) is a target curvature obtained at time k+1, Δρ is a difference value between the target curvatures obtained at time k+1 and time k; δ ff is a front wheel steering angle value calculated by a lateral preview feedforward controller; The calculation formulas of the final front wheel steering angle instruction and the final speed control instruction are as follows: δ f = δ mpc + δ ff v cmd = v p + v feed wherein: δ mpc is the lateral front wheel steering angle feedback value calculated by the model predictive closed-loop feedback controller; v feed is the speed feedback control variable in the model predictive closed-loop feedback controller; Step 7: Perform lateral and longitudinal collaborative control The final front wheel steering angle instruction and the final speed control instruction determined by step 6 are filtered and sent to the intelligent vehicle as control signals at a fixed frequency, and the intelligent vehicle performs horizontal and longitudinal collaborative control according to the final front wheel steering angle instruction and the final speed control instruction; further, it is judged whether the intelligent vehicle reaches the destination, if yes, the control task is completed, otherwise, step 1 is turned to.

Images