High-precision cooperative vehicle trajectory optimization optimal control method and system

Abstract

The invention relates to a high-precision cooperative vehicle trajectory optimization optimal control method and system, which are used for performing optimization control on a motion trajectory of a cooperative vehicle. The control system comprises a vehicle dynamics model, constraint conditions and a specified optimization target setting module. A cooperative vehicle MCU module; a vehicle travel distance sensor module; a vehicle speed sensor module; a distance sensor; a speed setting module of each cooperative vehicle; a vehicle acceleration control module; and all the modules are communicated through a data bus. The MCU executes a coherent optimization algorithm based on finite element orthogonal configuration according to the set initial position, initial speed, acceleration and speed constraint to obtain a track optimization control strategy, and the MCU converts the obtained control strategy into a control instruction and sends the control instruction to a vehicle acceleration controller to execute. According to the method, the control strategy of the vehicles can be quickly optimized according to information such as different positions and speeds of the vehicles in the vehicle row, the distance between the vehicles and the like, so that the comprehensive energy consumption of a multi-vehicle system is minimum.

Description

Technical Field

[0001] The present invention relates to the field of optimizing the motion trajectory of autonomous vehicles, and particularly to an optimal control method and system for optimizing the trajectories of highly precise cooperative vehicles. Under different initial states and external constraint conditions, cooperative vehicles can give the optimal control strategies for optimizing the motion trajectories of each cooperative vehicle, and convert them into acceleration control commands for vehicles. Under the condition of meeting the requirements of safe vehicle driving, the comprehensive energy consumption of the multi-vehicle system is minimized. Background Art

[0002] With the continuous development of China's economy and the continuous improvement of people's living standards, people's need for better transportation is growing day by day. As a basic means of transportation, the number of motor vehicles in China has shown a rapid growth trend. At the same time, the troubles brought by traffic jams and traffic accidents have become increasingly prominent. The inducing factors of road traffic accidents include weak road infrastructure, vehicle failures, and improper driver operations. Among them, improper operations caused by factors such as driver fatigue and emotional fluctuations are the primary reasons for major road traffic accidents. Therefore, improving the quality of driving behavior is the key to curbing the frequent occurrence of traffic accidents.

[0003] Autonomous vehicles have characteristics such as more sensitive environmental perception, more efficient behavior decision-making, and more stable control execution, which can effectively make up for the deficiencies of human drivers, alleviate the current situation of traffic congestion, and greatly reduce the number of traffic accidents. Therefore, autonomous driving technology has been highly regarded by countries around the world. With the in-depth application, the demand for autonomous driving has also become more diversified. A single autonomous vehicle cannot efficiently execute tasks with high parallelism and complexity, such as map exploration, formation transportation, formation patrol, disaster relief and rescue, etc. At this time, multiple vehicles need to work together.

[0004] As an important part of many core technologies for multi-vehicle cooperation in autonomous driving, trajectory planning generates the driving trajectories of each vehicle based on information such as the tasks of the vehicle and the surrounding environment. It is the most direct factor affecting the quality of vehicle driving behavior, and thus has received great attention and emphasis from scholars at home and abroad. However, the actual multi-vehicle cooperation trajectory planning model is complex and large-scale, the actual traffic scenarios are complex and changeable, and there are numerous constraint conditions. How to efficiently find the optimal path in the presence of obstacles and changing environments is the key to solving the problem of multi-vehicle cooperation trajectory planning in autonomous driving and a huge challenge faced. Therefore, the research on the method of multi-vehicle cooperation trajectory planning in autonomous driving is of great significance, which can improve the decision-making control level of autonomous driving and ensure the safety of road traffic. Summary of the Invention

[0005] To address the deficiencies in the existing technology, the present invention proposes a high-precision cooperative vehicle trajectory optimization optimal control system for optimizing the motion trajectories of cooperative vehicles. This control system consists of a vehicle speed sensor, a travel distance sensor, a vehicle microcontroller unit (MCU), and a vehicle acceleration controller. The vehicle MCU automatically executes a quasi-sequential optimization algorithm based on finite element orthogonal collocation according to the set initial position, initial speed, acceleration, and speed constraints, and obtains a trajectory optimization control strategy that minimizes the energy consumption of the multi-vehicle system. The vehicle MCU converts the obtained control strategy into a control instruction and sends it to the vehicle acceleration controller for execution.

[0006] Specifically, a high-precision cooperative vehicle trajectory optimization optimal control method of the present invention in a multi-vehicle system for cooperative autonomous driving includes the following steps: Step (1) Collect vehicle information; Step (2) Obtain a vehicle trajectory optimization control strategy that minimizes the comprehensive energy consumption of cooperative vehicles; Step (3) Convert the trajectory optimization control strategy into a control instruction and send it to the acceleration controller of the corresponding vehicle for execution.

[0007] In Step (2), the trajectory optimization control strategy is obtained by executing a quasi-sequential optimization algorithm based on finite element orthogonal collocation.

[0008] The goal of the trajectory optimization control strategy is to minimize the sum of the energy consumption of each cooperative vehicle in the multi-vehicle system. The derivation process of the mathematical model of the trajectory optimization control strategy is as follows:

[0009] The optimal control problem of cooperative vehicle trajectory optimization is described by the mathematical model as:

[0010]

[0011] x l (t 0 ) = x l,0

[0012]

[0013] u low ≤ u l (t) ≤ u up

[0014] t 0 ≤ t ≤ t f

[0015] l = 1, 2, …, L

[0016] Where:

[0017] t represents time, t 0 represents the initial time of cooperative vehicle trajectory optimization, t f represents the end time of cooperative vehicle travel and tf Not fixed;

[0018] is the state vector, n x is the dimension of the state vector, x l,0 is the initial value of the state vector, is the first derivative of the state vector;

[0019] u l (t) represents the acceleration of the cooperative vehicle and is also the control variable of the optimal control problem, u low 、u up respectively represent its lower limit value and upper limit value;

[0020] is a mathematical model of a differential equation system established based on the vehicle motion equation, that is:

[0021] G l [u l (t), x l (t), t] are the constraints during the motion of the cooperative vehicle, l is the index of the vehicle number, and L represents the number of cooperative vehicles;

[0022] Then the mathematical model for minimizing the comprehensive energy consumption is expressed as:

[0023]

[0024] x l (t 0 ) = x l,0

[0025]

[0026] u low ≤ u l (t) ≤ u up

[0027] t 0 ≤ t ≤ t f

[0028] l = 1, …, L

[0029] Where:

[0030] J[u 1 (t), …, u L (t)] represents that the objective function of the cooperative vehicle trajectory optimization is the energy consumption of all vehicles at the end of the optimization. The objective function J is determined by the acceleration u 1 (t), …, u L (t);

[0031] f l(v l (t)) represents the instantaneous power of the l-th vehicle, which is a function of its speed v l (t):

[0032] f l (v l (t)) = b 0 + b 1 v l (t)+ b 2 v l 2 (t)+ b 3 v l 3 (t)

[0033] where: b 0 , b 1 , b 2 , b 3 are constants;

[0034] In step (i), the information of each cooperative vehicle in the multi-vehicle system is collected; for any cooperative vehicle, the collected information includes: the instantaneous position and instantaneous speed of the vehicle; the set position and set speed of the vehicle (referring to the position and speed of the vehicle at the end of optimization. Usually, the preset desired position and desired speed); the initial position and initial speed of the vehicle; the distance between vehicles; the dynamic model f l (t, x l (t), u(t)), the constraint condition G l [u l (t), x l (t), t] and the specified optimization target parameters;

[0035] In step (ii), the steps of the quasi-sequential optimization algorithm based on finite element orthogonal collocation include:

[0036] 2.1) Before the cooperative vehicle moves, the speed sensors, travel distance sensors, and distance sensors of each vehicle are turned on to collect the position and speed state information of each cooperative vehicle at the initial moment;

[0037] 2.2) Set the number of discrete segments of the trajectory optimization process time, set the initial guess value of the control quantity / acceleration u(t) as u (0) (t), and set the optimization accuracy requirement tol; t represents time;

[0038] 2.3) Discretize all the ordinary differential equations on the time axis [t 0 , t f through finite element orthogonal collocation of ordinary differential equations;

[0039] 2.4) Divide the discrete variables in step 2.3) into free variables and non-free variables through quasi-order to form a non-linear programming NLP problem;

[0040] 2.5) Solve through the sensitivity matrix to calculate the first-order sensitivity information of non-free variables with respect to free variables, and calculate the gradient information of the objective function and constraint conditions with respect to free variables;

[0041] 2.6) Solve the NLP problem to obtain the required acceleration control strategy and the corresponding state trajectory. This process includes multiple internal iterations; for the control quantity u (k) (t) obtained in the k-th iteration, if its corresponding objective function value J[u (k) (t)] and the objective function value J[u (k-1) (t)] of the previous iteration k - 1 differ by less than the accuracy requirement tol, then the internal iteration process ends; otherwise, continue to the next iteration.

[0042] In step 2.3), the steps include:

[0043] 2.3.1) Approximate the control quantity / acceleration u(t) with piecewise constants, and approximate the state trajectory x(t) with an M-order Lagrange interpolation polynomial, i.e.:

[0044] u(t)≈u i i = 1, 2,..., N(1)

[0045]

[0046] where: t represents time, N is the number of segments for discretizing the time interval [t 0 , t f , is the Lagrange interpolation basis function, u i is the discretized control parameter, s i,j is the parameter value of x(t) at the Gaussian collocation point t i,j ;

[0047] 2.3.2) Differentiate formula (2) to obtain an approximate expression for the derivative of the state variable:

[0048]

[0049] 2.3.3) Discretize the differential equation system of the state trajectory into an algebraic equation form, and discretely express the objective function and constraint conditions using u i and s i,j .

[0050] In step 2.4), the steps include:

[0051] 2.4.1) Let \(U = [u 1 , u 2 , \ldots, u N \) T be the parameters of all control variables, where the superscript \(T\) represents the transpose of a vector or matrix, \(t f is the motion time of the cooperative vehicle, \(X = [s 1,1 , s 1,2 , \ldots, s N,M \) T be the parameters of the vehicle state; regard \(U\) and \(t f as free variables and \(X\) as non - free variables;

[0052] 2.4.2) In the formed NLP problem, \(U\) and \(t f are variables to be optimized. The model of the NLP problem is as follows:

[0053]

[0054] Where:

[0055] \(J\) represents the objective function, which is a function of the state parameter \(X(U, t f ),\) the control parameter \(U\), and the time parameter \(t f . \(\min\) represents minimizing the objective function;

[0056] \(X(U, t f )\) indicates that \(X\) is jointly determined by \(U\) and \(t f ;

[0057] \(C E \) represents all algebraic equality constraints, and \(C I \) is the algebraic inequality constraint;

[0058] s.t. indicates that the programming problem is restricted by \(C E \) and \(C I .

[0059] In the above - mentioned 2.5), it includes the steps:

[0060] 2.5.1) Define the first - order sensitivity information \(S i \) of the state parameter \(X\) of the \(i\) - th segment with respect to the control parameter \(U j \) of the \(j\) - th segment, and define the first - order sensitivity information X,U of the state parameter \(X i with respect to the time parameter \(t f \) as follows: As follows:

[0061]

[0062] Where: Denotes taking partial derivative; the $i$-th and $j$-th segments refer to time periods. The time interval $[t_0, t_f]$ is divided into $N$ segments, and the $i$-th and $j$-th segments are certain segments among them;

[0063] 2.5.2) Write the equation $F$ discretized from the differential equation system i in the following form:

[0064] $F$ i $(X$ i , $U$ j , $t$ f , $X$ i-1 , …, $X$ 1 ) = 0 (7)

[0065] Take partial derivatives with respect to $U$ j and $t$ f in Equation (7),

[0066]

[0067] 2.5.3) Calculate the first-order sensitivity information $S$ X,U and

[0068]

[0069] 2.5.4) Calculate the gradient information of the NLP problem according to Equations (10) and (11):

[0070]

[0071] A high-precision cooperative vehicle trajectory optimization optimal control system includes: a vehicle dynamics model, constraint conditions, a specified optimization target setting module; a cooperative vehicle MCU module; a vehicle driving distance sensor module; a vehicle speed sensor module; a distance sensor; a speed setting module for each cooperative vehicle; a vehicle acceleration control module; each module communicates through a data bus;

[0072] Install the control system on each cooperative vehicle in the multi-vehicle system; then for any cooperative vehicle, the operation process of the motion system is as follows:

[0073] Step 1): Input the vehicle dynamics model, constraint conditions during the motion process, and specified optimization target parameter information into the cooperative vehicle MCU module;

[0074] Step 2): At a certain moment, turn on the vehicle speed sensor, driving distance sensor, and distance sensor to obtain the vehicle's current position, speed, and distance information from other cooperative vehicles;

[0075] Step 3): The cooperative vehicle MCU module executes a quasi-sequential optimization algorithm based on finite element orthogonal collocation according to the speed settings of each cooperative vehicle, the vehicle dynamics model, the constraint conditions, and the specified optimization objectives, and obtains a trajectory optimization control strategy that minimizes the comprehensive energy consumption of the cooperative vehicles;

[0076] Step 4): The cooperative vehicle MCU module converts the obtained trajectory optimization control strategy into a control instruction and sends it to the vehicle acceleration controller of the vehicle itself;

[0077] The quasi-sequential optimization algorithm based on finite element orthogonal collocation is the aforementioned quasi-sequential optimization algorithm.

[0078] The present invention can quickly optimize and obtain the control strategy of the vehicle according to information such as the different positions, speeds, and distances between vehicles in the vehicle platoon, so as to minimize the comprehensive energy consumption of the multi-vehicle system.

[0079] The beneficial effects of the present invention are mainly manifested in that: the high-precision cooperative vehicle trajectory optimization optimal control system can efficiently obtain the optimal control strategies and motion trajectories of each cooperative vehicle under different initial conditions, driving conditions, and target tasks in view of the characteristics of large scale and complex model of the trajectory planning problem during multi-vehicle cooperation. Under the condition of meeting the requirements of safe vehicle driving, the comprehensive energy consumption of the multi-vehicle system is minimized, the driving behavior quality of the cooperative vehicles is improved, and the level of autonomous driving decision-making control is improved. Description of the Drawings

[0080] Figure 1 is a schematic structural diagram of a high-precision cooperative vehicle trajectory optimization optimal control system;

[0081] Figure 2 is a structural diagram of the internal module of the MCU of the high-precision cooperative vehicle trajectory optimization optimal control system. Detailed Embodiments

[0082] In order to improve the driving quality of cooperative vehicles, improve the level of autonomous driving decision-making control, and reduce the comprehensive energy consumption of cooperative vehicles, the present invention provides a high-precision cooperative vehicle trajectory optimization optimal control method and system with high computational efficiency and capable of meeting various types of constraint conditions. The present invention uses the MCU as the implementation carrier of the optimal control method.

[0083] The optimal control problem of cooperative vehicle trajectory optimization can be described by a mathematical model as follows:

[0084]

[0085] Where:

[0086] t represents time, t 0 represents the initial time of the cooperative vehicle trajectory optimization problem, t frepresents the end time of cooperative vehicle driving, and t f is not fixed;

[0087] is the state vector, n x is the dimension of the state vector, x l,0 is the initial value of the state vector, is the first derivative of the state vector;

[0088] u l (t) represents the acceleration of the cooperative vehicle and is the control variable of this problem. u low , u up represent its lower limit value and upper limit value respectively;

[0089] is a system of differential equations established based on mechanical principles;

[0090] G l [u l (t), x l (t), t] are the constraint conditions during the movement of the cooperative vehicle. l is the index of the vehicle number, and L represents the number of cooperative vehicles.

[0091] For the optimal control problem of cooperative vehicle trajectory optimization, the mathematical model that minimizes the comprehensive energy consumption can be expressed as:

[0092]

[0093] Among them:

[0094] J[u 1 (t), …, u L (t)] indicates that the objective function J is determined by the acceleration control variables u 1 (t), …, u L (t).

[0095] f l (v l (t)) represents the instantaneous power of the l-th vehicle and is a function of its speed v l (t):

[0096]

[0097] Among them: b 0 , b 1 , b 2 , b 3 are constants. This problem is essentially an optimal control problem.

[0098] The technical solution adopted by the present invention to solve its technical problems is as follows: a quasi-sequential optimization algorithm based on finite element orthogonal collocation is integrated in the cooperative vehicle micro control unit (MCU). Under different initial states and external constraint conditions, the optimal control instructions for the cooperative vehicle motion trajectory can be given, so that the comprehensive energy consumption of the cooperative vehicle is minimized. Specifically:

[0099] The structural schematic diagram of the high-precision cooperative vehicle trajectory optimization optimal control system of the present invention is as Figure 1 shown, including:

[0100] Vehicle dynamics model, constraint conditions, and specified optimization target setting module 11,

[0101] Cooperative vehicle MCU module 12,

[0102] Vehicle driving distance sensor module 13,

[0103] Vehicle speed sensor module 14,

[0104] Distance sensor 15,

[0105] Speed setting module 16 for each cooperative vehicle,

[0106] Vehicle acceleration control module 17,

[0107] All components / modules within the system are connected by the data bus within the control system.

[0108] The operation process of the high-precision cooperative vehicle trajectory optimization optimal control system of the present invention is as follows:

[0109] Step 1): Install the control system on the vehicle (i.e., the vehicle itself), and input the dynamic model of each cooperative vehicle, the constraint conditions during the movement process, and the specified optimization target parameter information 11 into the cooperative vehicle MCU module 12;

[0110] Step 2): At a certain moment, turn on the speed sensors 14, driving distance sensors 13, and distance sensors 15 of each vehicle to obtain information such as the current position, speed of the vehicle, and the distance from other vehicles;

[0111] Step 3): The cooperative vehicle MCU module 12 automatically executes the quasi-sequential optimization algorithm based on finite element orthogonal collocation inside the MCU module according to the speed setting of each cooperative vehicle, the vehicle dynamics model, the constraint conditions, and the specified optimization target, and obtains the trajectory optimization control strategy that minimizes the comprehensive energy consumption of the cooperative vehicle;

[0112] Step 4): The cooperative vehicle MCU module 12 converts the obtained trajectory optimization control strategy into control instructions and sends them to the vehicle acceleration controller 17 of the vehicle itself;

[0113] The collaborative vehicle MCU module integrated with the quasi-sequential optimization algorithm based on finite element orthogonal collocation is the core of the present invention. As Figure 2 shown, it internally includes an information acquisition module 21, an initialization module 22, a finite element orthogonal collocation module 23 for ordinary differential equations (ODEs), a quasi-sequential module 24, a sensitivity matrix solving module 25, a non-linear programming (NLP) problem solving module 26, and a control instruction output module 27. Among them:

[0114] 1. Information acquisition module 21: It is used to collect the position and speed information of the current vehicle, collect the vehicle position and speed setting information, collect the initial position and speed of the vehicle, collect the spacing information between vehicles, and collect the vehicle dynamics model, constraint conditions, and specified optimization target parameter information.

[0115] 2. The collaborative vehicle MCU module 12 uses the quasi-sequential optimization algorithm based on finite element orthogonal collocation to automatically generate acceleration control instructions. The algorithm running steps are as follows:

[0116] Step 1): Before the collaborative vehicle moves, the vehicle speed sensor, travel distance sensor, and distance sensor are turned on, and the information acquisition module 21 obtains the position and speed state information of the collaborative vehicle at the initial moment.

[0117] Step 2): The initialization module 22 starts to run, sets the number of discrete segments of the trajectory optimization process time, sets the initial guess value u (0) (t) of the control quantity (i.e., acceleration), and sets the optimization accuracy requirement tol.

[0118] Step 3): Through the ODE finite element orthogonal collocation module 23, the ordinary differential equations are all discretized on the time axis [t 0 , t f .

[0119] Step 4): Through the quasi-sequential module 24, the discrete variables in Step 3) are divided into free variables and non-free variables to form an NLP problem.

[0120] Step 5): Through the sensitivity matrix solving module 25, calculate the first-order sensitivity information of non-free variables to free variables, and calculate the gradient information of the objective function and constraint conditions to free variables.

[0121] Step 6): Through the NLP problem solving module 26, obtain the required acceleration control strategy and corresponding state trajectory, and this process includes multiple internal iterations. For the control quantity u (k) (t) obtained in the k-th iteration, if its corresponding objective function value J[u(k) (t)] The difference between the objective function value J[u (k-1) (t)] in the previous iteration k - 1 and the current one is less than the precision requirement tol, then the internal iteration process ends; otherwise, continue to the next iteration.

[0122] Step 7): Convert the acceleration control strategy into a control instruction through the control instruction output module 27 and input it into the vehicle acceleration controller.

[0123] 3. The ODE finite element orthogonal collocation module 23 is implemented as follows:

[0124] Step 1): Approximate the acceleration (control variable) u(t) with piecewise constants, and approximate the state trajectory x(t) with an M - order Lagrange interpolation polynomial, i.e.:

[0125] u(t)≈u i i = 1,2,...,N(1)

[0126]

[0127] where: t represents time, N is the number of segments for discretizing the time interval [t 0 ,t f , is the Lagrange interpolation basis function, u i are the discretized control parameters, s i,j are the parameter values of x(t) at the Gaussian collocation points t i,j .

[0128] Step 2): Differentiate formula (2) to obtain an approximate expression for the derivative of the state variable:

[0129]

[0130] Step 3): Discretize the differential equation system of the state trajectory into an algebraic equation form, and discretely express the objective function, constraints, etc. using u i and s i,j .

[0131] 4. The quasi - sequential module 24 is implemented as follows:

[0132] Step 1): Let U = [u 1 ,u 2 ,…,u N T be the parameters of all control variables, where the superscript T represents the transpose of a vector or matrix, t f is the motion time of the cooperative vehicle, X = [s 1,1 ,s 1,2 ,…,s​N,M T is a parameter of the vehicle state. Regarding U and t f as free variables and X as a non-free variable.

[0133] Step 2): In the formed NLP problem, U and t f are variables to be optimized. The model of the NLP problem is as follows:

[0134]

[0135] where: J represents the objective function, which is a function of the state parameter X(U, t f ), the control parameter U, and the time parameter t f . min represents minimizing the objective function. X(U, t f ) indicates that X is jointly determined by U and t f . C E represents all algebraic equality constraints, and C I is the algebraic inequality constraint. s.t. indicates that the programming problem is subject to C E and C I .

[0136] 5. The sensitivity matrix solving module 25 is implemented by the following steps:

[0137] Step 1): Define the first-order sensitivity information S i of the state parameter X of the i-th segment j with respect to the control parameter U of the j-th segment X,U , and the first-order sensitivity information i of the state parameter X with respect to the time parameter t f as follows: as follows:

[0138]

[0139] where: represents taking the partial derivative.

[0140] Step 2): Write the equation F i discretized from the differential equation system in the following form:

[0141] F i (X i , U j , t f , X i-1 , …, X 1 ) = 0 (7)

[0142] Take the partial derivatives of equation (7) with respect to U j and t f respectively. ​

[0143]

[0144] Step 3): Calculate the first-order sensitivity information S respectively through equations (8) and (9) X,U and

[0145]

[0146] Step 4): Calculate the gradient information of the NLP problem according to equations (10) and (11):

[0147]

[0148] Finally, the cooperative vehicle MCU converts the obtained optimized control trajectory through the control instruction output module 27 into a control instruction and sends it to the vehicle acceleration controller to complete the execution of the trajectory optimization.

[0149] The present invention will be further described below in conjunction with specific embodiments.

[0150] In this embodiment, before the vehicle travels, the driving distance sensors, vehicle speed sensors, distance sensors and MCUs in the multi-vehicle system are all turned on. The information acquisition module immediately acquires the initial position and speed of the vehicle.

[0151] Suppose there are five vehicles traveling in cooperation, that is, L = 5, and the current initial time t 0 = 0 s, and the speed v 1,0 = v 2,0 = … = v 5,0 = 0 m / s transmitted by the speed sensor of each vehicle to the MCU.

[0152] The first vehicle is at the frontmost position in the driving direction, and the initial position transmitted to the MCU is x 1,0 = 0 m. The distance between the l-th (l ≥ 2) vehicle and the previous (l - 1)-th vehicle is 8 meters. Then the initial positions of the vehicles transmitted to the MCU are x 1,0 = -8(l - 1) m; at the termination time t f , the conditions that each vehicle needs to meet are set as v l (t f ) = 22.2 m / s. Combining the vehicle dynamics model, constraint conditions, and specified optimization target parameters, the mathematical model of this problem is as follows:

[0153]

[0154] x l (0) = x l,0 = -8(l - 1), l = 1, …, L

[0155] vl v(0)=v l,0 =0, l = 1, …, L

[0156] x l (t) - x l-1 (t) ≥ 5, l = 2, …, L

[0157] u low ≤ u l (t) ≤ u up

[0158] v l (t f ) = v l,f

[0159] t 0 ≤ t ≤ t f

[0160] Among them, L = 5 represents the number of cooperative vehicles, v l,f = 22.2 m / s is the speed of the vehicle at the end of optimization (at time t f ), u low = -2.31 m 2 / s, u up = 2.31 m / s 2 respectively represent the lower and upper bounds of the vehicle acceleration, represents the energy consumption of the l-th vehicle per unit time, where b 0 = 0.156, b 1 = 2.450×10 -2 , b 2 = -7.415×10 -4 , b 3 = 5.975×10 -5 .

[0161] For the sake of convenience of expression, use f l (t, x l (t), u(t)) to represent the mathematical model of the differential equation system established according to the vehicle motion equation, that is:

[0162]

[0163] Use G l to represent the constraint conditions during the vehicle driving process, which are:

[0164]

[0165] In addition, J[u 1 (t), …, u L (t)] represents that the objective function of the cooperative vehicle trajectory optimization is the energy consumption of all vehicles at the end of optimization.

[0166] Referring to Figure 2 , the optimal control system for high-precision cooperative vehicle trajectory optimization in this example includes:

[0167] 1. An information acquisition module, which is used to acquire the position and speed information of the current vehicle, acquire the vehicle position and speed setting information, acquire the initial position and speed of the vehicle, acquire the spacing information between vehicles, and acquire the vehicle dynamics model, constraint conditions, and specified optimization target parameter information. Before the vehicle travels, the vehicle travel distance sensor, vehicle speed sensor, distance sensor, and MCU are all turned on. The information acquisition module 21 obtains the initial speed v of each vehicle at the initial moment t 0 = 0 s as v 1,0 = v 2,0 = … = v 5,0 = 0 m / s and the initial position x 1,0 = -8(l - 1) m, where l = 1, …, 5. At the termination moment t f , the speed of the vehicle is set to v l (t f ) = 22.2 m / s;

[0168] 2. A cooperative vehicle MCU module, which adopts a quasi-sequential optimization algorithm based on finite element orthogonal collocation to automatically generate acceleration control instructions. The algorithm running steps are as described above.

[0169] Finally, the cooperative vehicle MCU converts the obtained optimized control trajectory into a control instruction through the control instruction output module and sends it to the vehicle acceleration controller to complete the execution of the trajectory optimization.

[0170] The above content is a further detailed description of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is only limited to these descriptions. For those of ordinary skill in the technical field to which the present invention belongs, without departing from the inventive concept, several simple deductions or substitutions can still be made, which should all be regarded as belonging to the protection scope of the present invention.

Claims

1. A high-precision coordinated vehicle trajectory optimization optimal control method, in a multi-vehicle autonomous driving system in which multiple vehicles are coordinated, comprising the following steps: step (i) collecting vehicle information; step (ii) obtaining a vehicle trajectory optimization control strategy that minimizes the comprehensive energy consumption of the coordinated vehicles; step (iii) converting the trajectory optimization control strategy into a control instruction and sending it to the acceleration controller of the corresponding vehicle for execution; Its characteristic is that In step (ii), the trajectory optimization control strategy is obtained by executing a quasi-sequential optimization algorithm based on finite element orthogonal configuration; The goal of the trajectory optimization control strategy is to minimize the sum of the energy consumption of each coordinated vehicle in the multi-vehicle system. The mathematical model derivation process of the trajectory optimization control strategy is as follows: The optimal control problem of cooperative vehicle trajectory optimization is described by a mathematical model: x l (t0)=x l,0 u low ≤u l (t)≤u up t0≤t≤t f l＝1,2,…,L in: t represents time, t0 represents the initial time of the coordinated vehicle trajectory optimization, t f represents the end time of the cooperative vehicle driving and t f Not fixed; is the state vector, n x is the dimension of the state vector, x l,0 is the initial value of the state vector, is the first-order derivative of the state vector; u l (t) represents the acceleration of the cooperative vehicle and is also the control variable of the optimal control problem. low 、u up Respectively represent its lower limit and upper limit; It is a mathematical model of a differential equation system established based on the vehicle motion equation, namely: G l [u l (t),x l (t), t] are the constraints during the movement of the cooperative vehicles, l is the index of the vehicle sequence number, and L represents the number of cooperative vehicles; The mathematical model that minimizes the comprehensive energy consumption is expressed as: x l (t0)=x l,0 u low ≤u l (t)≤u up t0≤t≤t f l＝1,…,L in: J[u1(t),…,u L (t)] indicates that the objective function of the coordinated vehicle trajectory optimization is the energy consumption of all vehicles at the end of the optimization. The objective function J is composed of the accelerations u1(t),…,u L (t)determine; f l (v l (t)) represents the instantaneous power of the lth vehicle, which is its speed v l Function of (t): Among them: b0, b1, b2, b3 are constants; In step (1), information of each coordinated vehicle in the multi-vehicle system is collected; for any coordinated vehicle, the collected information includes: the vehicle's instant position and instant speed; the vehicle's set position and set speed; the vehicle's initial position and initial speed; the distance between each vehicle; the vehicle's dynamic model f l (t,x l (t),u(t)), constraint G l [u l (t),x l (t),t] and specify the optimization target parameters; In step (ii), the steps of the quasi-sequential optimization algorithm based on finite element orthogonal configuration include: 2.1) Before the coordinated vehicles move, the speed sensors, driving distance sensors, and distance sensors of each vehicle are turned on to collect the position and speed status information of each coordinated vehicle at the initial moment; 2.2) Set the number of discrete segments of trajectory optimization process time and the initial guess value of control quantity / acceleration u(t) to u (0) (t), sets the optimization accuracy requirement tol; t represents time; 2.3) Through the ODE finite element orthogonal configuration of the ODE system, the ODE system is transformed on the time axis [t0,t f ] are all discrete; 2.4) The discrete variables in step 2.3) are divided into free variables and non-free variables by quasi-sequential method, forming a nonlinear programming NLP problem; 2.5) By solving the sensitivity matrix, calculate the first-order sensitivity information of non-free variables to free variables, and calculate the gradient information of objective function and constraint conditions to free variables; 2.6) Obtain the required acceleration control strategy and corresponding state trajectory through NLP problem solving. This process includes multiple internal iterations. For the control quantity u obtained in the kth iteration, (k) (t), if its corresponding objective function value J[u (k) (t)] and the objective function value J[u (k-1) (t)] is less than the accuracy requirement tol, the internal iteration process ends, otherwise it continues to the next iteration.

2. The high-precision cooperative vehicle trajectory optimization optimal control method according to claim 1 is characterized by: The step 2.3) includes the following steps: 2.3.1) The control quantity / acceleration u(t) is approximated by a piecewise constant, and the state trajectory x(t) is approximated by an M-order Lagrange interpolation polynomial, that is: u(t)≈u i i=1,2,...,N (1) Where: t represents time, N represents the time interval [t0,t f ] to discretize the number of segments, is the Lagrange interpolation basis function, u i is the discretized control parameter, s i,j is x(t) at the Gaussian collocation point t i,j Parameter values ​​on ; 2.3.2) Deriving formula (2), we can get the approximate expression of the derivative of the state variable: 2.3.3) Discretize the differential equations of the state trajectory into algebraic equations, and use u i and i,j Discrete expression.

3. The high-precision cooperative vehicle trajectory optimization optimal control method according to claim 1 is characterized by: The step 2.4) includes the following steps: 2.4.1) Let U=[u1,u2,…,u N ] T are the parameters of all control quantities, where the superscript T represents the transpose of a vector or matrix, and t f is the movement time of the cooperative vehicle, X = [s 1,1 ,s 1,2 ,…,s N,M ] T is the parameter of vehicle state; U and t f Treat as a free variable and X as a non-free variable; 2.4.2) In the NLP problem, U and t f As the variable to be optimized, the model of the NLP problem is as follows: in: J represents the objective function, which is the state parameter X(U,t f ), control parameter U, time parameter t f function, min means minimizing the objective function; X(U,t f ) indicates that X is composed of U and t f Joint determination; C E represents all algebraic equality constraints, C I is an algebraic inequality constraint; st indicates that the planning problem is subject to C E and C I restrictions.

4. The high-precision cooperative vehicle trajectory optimization optimal control method according to claim 1 is characterized by: The above 2.5) includes the steps of: 2.5.1) Define the state parameter X of the i-th segment i The control parameter U for the jth segment j The first-order sensitivity information S X,U , define the state parameter X i For the time parameter t f First-order sensitivity information As shown below: in: Indicates partial derivative; the i-th and j-th segments refer to time periods, and the time interval [t0,t f ] is divided into N sections, and the i-th section and the j-th section are one of them; 2.5.2) Discretize the equation F from the differential equation system i Written in the following form: F i (X i ,U j ,t f ,X i-1 ,…,X1)=0 (7) In formula (7), U j and t f To find partial derivatives, 2.5.3) Calculate the first-order sensitivity information S by equations (8) and (9) respectively X,U and 2.5.4) Calculate the gradient information of the NLP problem according to equations (10) and (11):

5. A high-precision cooperative vehicle trajectory optimization optimal control system, characterized by include: Vehicle dynamics model, constraints, specified optimization target setting module; collaborative vehicle MCU module; Vehicle distance sensor module; Vehicle speed sensor module; distance sensor; Speed ​​setting module for each coordinated vehicle; vehicle acceleration control module; each module communicates via a data bus; The control system is installed on each cooperative vehicle in the multi-vehicle system; then for any cooperative vehicle, the operation process of the motion system is as follows: Step 1): Input the dynamic model of the cooperative vehicle, the constraints during the movement, and the specified optimization target parameter information into the cooperative vehicle MCU module; Step 2): At a certain moment, turn on the vehicle's speed sensor, driving distance sensor, and distance sensor to obtain the vehicle's current position, speed, and distance information with other coordinated vehicles; Step 3): The cooperative vehicle MCU module executes a quasi-sequential optimization algorithm based on finite element orthogonal configuration according to the speed setting, vehicle dynamics model, constraints, and specified optimization objectives of each cooperative vehicle to obtain a trajectory optimization control strategy that minimizes the comprehensive energy consumption of the cooperative vehicle; Step 4): The collaborative vehicle MCU module converts the obtained trajectory optimization control strategy into a control command and sends it to the vehicle acceleration controller of the vehicle; The quasi-sequential optimization algorithm based on finite element orthogonal configuration is the quasi-sequential optimization algorithm described in any one of claims 1 to 4.

Images